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ABSTRACT 


The middle, or Tactical, level of the Rational Behavior Model (RBM) is the essential 
bridge linking the top and bottom levels of the model together. To insure an autonomous 
vehicle maintains control and thus exhibits rational behavior during such time-consuming 
tasks as search, homing, and route replanning, the Tactical level must be able to handle 
concurrency. Until now, this level has been implemented in only a limited way using an 
object-oriented language and sequential operations. The objective of this thesis is to 
construct an implementation model that represents the concurrency inherent in the Tactical 
level within the framework of the design model already developed. 

The method for building this implementation is to use the Ada task construct for 
concurrency to represent the objects of the design model and their communication with 
each other. 

This research creates a Tactical level implementation in Ada for the NPS Autonomous 
Underwater Vehicle (AUV) simulator that successfully executes a mission scenario 
involving transit, search, task, and return phases and the same mission scenario with route 
replanning. This work thus provides a foundation for future development of concurrent 
implementations of this level of RBM. 
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I. INTRODUCTION 


A. BACKGROUND 

Controlling autonomous vehicles through software is a challenging area of software 
engineering requiring a variety of resources. Neither completely relying on a single 
programming paradigm nor simply throwing together all available programming resources 
can provide the long-term stability necessary for an autonomous vehicle software system. 
A software architecture with multiple levels of abstraction is extremely important for 
handling the complexity of autonomous operations in the real world. Such an architecture 
provides for the use of specific programming paradigms to address particular levels of a 
problem. Reliability and maintainability of software then become key factors in 
determining the applicability of a programming paradigm to a certain level of abstraction, 
and they are built into the system instead of being produced incidentally. 

To model the real world, autonomous vehicle software systems need to be capable of 
managing concurrency. Events, and thus behaviors, in the real world are neither sequential 
in time nor centralized in a single, physical entity. Concurrency involves the twin issues of 
multitasking, in which a single entity performs multiple operations at the same time, and 
distribution, in which many entities perform separate tasks simultaneously. In addition, 
reuse of software is very desirable in this complex development environment. The object- 
oriented programming paradigm with its built-in inheritance mechanism facilitates the 
reuse of existing implementations [Kwak90] [Toml89]. The capability to implement a 
concurrent, object-oriented solution is a powerful tool in accurately modeling the problem 
domain and an effective weapon in battling against software complexity. 

B. STATEMENT OF THE PROBLEM 

The Rational Behavior Model (RBM) is a multi-level, multi-paradigm software 
architecture for the control of autonomous vehicles. The top, or Strategic, level consists of 
general mission directives and the bottom, or Execution, level consists of specific vehicle 
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commands [Bym93]. Both have been specified and implemented in some detail. The 
middle, or Tactical, level, is responsible for breaking down the broad guidance of the 
Strategic level into simple pieces of behavior that the Execution level can carry out This 
level is thus the crucial bridge that connects the other two distinct parts of the model, but it 
has been implemented in only a very limited way. 

The design of the Tactical level is well-suited to the object-oriented paradigm and has 
been described in [Bym93]. The behaviors of the Tactical level can be grouped together 
quite easily under objects in an object hierarchy. Implementing the relationships of this 

hierarchy requires an object-oriented or object-based language 1 . The complex, time- 
consuming nature of certain tasks such as search, homing, and mission replanning make 
concurrent programming facilities extremely desirable as well so that control of the vehicle 
can be maintained continuously throughout a mission, insuring the vehicle’s rational 
behavior. Therefore, the problem is to find a programming language to represent the 
concurrency and the object-oriented nature of the Tactical level well and to build an 
implementation model. 

C. SCOPE 

The primary goal of this research is to develop a working model of the Tactical level 
of RBM in a currently available programming language using object-oriented techniques 
and programming language constructs for concurrency. For this research, concurrency is 
limited to multitasking, or the interleaving of multiple processes on a single processor. 
Distribution is beyond the scope of this work. This thesis focuses on a few areas of research, 
including representing concurrency in software, implementing object-oriented design, and 
the suitability of current programming languages for these two tasks. 


1. Object-based languages have features to support the principles of data abstraction and informa¬ 
tion hiding, while object-oriented languages have mechanisms for inheritance, dynamic binding, 
and polymorphism in addition to those features. However, as Booch notes, “... it is possible and 
highly desirable for us to use object-oriented design methods for both object-based and object-ori¬ 
ented programming languages.” [Booc91, p. 36] 
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D. THESIS ORGANIZATION 

Chapter II surveys previous work on software systems that have implemented object- 
oriented design and concurrency. Chapter III gives an overview of RBM. Chapter IV 
discusses the programming languages considered for implementing the Tactical level. In 
Chapter V, the Tactical level implementation is explained in detail. Chapter VI examines 
testing of the implementation in the laboratory on the AUV simulator. Chapter VII provides 
a summary of conclusions and suggestions for future research. Appendix A lists the source 
code for ti e Tactical level. Appendix B gives a trace of the execution of two multi-phase 
mission scenarios. Appendix C is a user’s guide to the AUV simulator used in this research. 
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n. PREVIOUS WORK 


A. INTRODUCTION 

There have been numerous efforts to implement concurrency using multi-tasking in 
real-time software applications. Three projects with varying timing requirements are 
described here. All three projects have employed some form of the Ada programming 
language and have either attempted to use or intend to use Ada’s task construct for 
concurrency. 

B. NASA OMV 

NASA’s Orbital Maneuvering Vehicle (OMV) is a semi-autonomous spacecraft 
designed to provide services to other spacecraft, including delivery, retrieval, reboosting, 
and deboosting. The craft has automatic navigation and rendezvous capabilities but 
requires human control for terminal operations such as docking with NASA’s Space 
Station. Control for the OMV can be provided from the space shuttle, from the ground, or 
from the Space Station. The OMV can carry various mission kits and has a nine month on- 
orbit capability. 

Standard Ada was used for prototyping on the software system. Tasking was rejected 
for this system, however, due to the system’s strict real-time requirements. In particular, the 
need to change the priority of a task at run time and the need to specify a task as non- 
preemptible by other tasks to meet certain time constraints were seen as necessary features 
not provided by the Ada Run Time System (RTS). Prototype tasking algorithms were much 
slower and larger than the established sequential ones. As a result, Ada tasking was not 
used further in the project [Howl88]. 

C. NASA EXPLORER MMS 

NASA’s Explorer Multimission Modular Spacecraft (MMS) is an unmanned orbiting 
space vehicle with a replaceable payload. The payload is a science instrument replaced by 
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the space shuttle every 18 to 24 months. Control of Explorer, such as attitude commands 
are generated by the ground, the onboard processor, or the onboard coprocessor. 

Standard Ada was used in a benchmark test with the intent of seeing how it would 
handle some of the spacecraft’s software functions, including attitude determination 
support, coprocessor system monitoring, and coprocessor self-checks. Developers 
considered tasking viable for this system with some changes in the task scheduler to reduce 
overhead time. Published task rendezvous time of 800 microseconds was not critical for 
this implementation. What was important was that task priorities could be set and 
synchronous and asynchronous interrupts handled due to minimal human control 
(Communication with the ground is limited to about 15 minutes every 1 1/2 hours). Planned 
modifications to the Ada RTS were designed to identify the cause of an interrupt and the 
portion of code involved in a telemetry report to the ground [Scot88], 

D. NAVAL POSTGRADUATE SCHOOL AUV 

1. Vehicle Description 

The Naval Postgraduate School Autonomous Vehicle (AUV) is an unmanned, 
untethered, robotic submarine. Its purpose is to provide multi-area research for students and 
faculty and its projected missions include search, surveillance, mapping and intervention 
activities. The current model of the vehicle, shown in Figure 1, is 7 feet long, weighs 
approximately 400 pounds, and has a maximum speed of 2 knots. Due to its relatively small 
size and low cost, the vehicle is an ideal research platform. Power for control surfaces and 
cross-body thrusters is provided by a battery-based system which can last 2 to 3 hours on a 
charge. The vehicle is controlled by two separate processors on Gespac platforms: one for 
vehicle actuator control and one for mission control and navigation. Sonar, inertial 
navigation, and global positioning systems are also incorporated onboard [Heal92]. 

Software control is provided by RBM, which is described in Chapter III. The 
high-level navigation and system-monitoring functions comprise the Tactical level. Byrnes 
in [Bym93] developed a Tactical level instantiation using Classic-Ada, a preprocessor for 
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68030/OS-9 
Vehicle Control 
Computer 


80386/MS-DOS 
Mission Control 
Computer 



Side View 


Figure 1 The Naval Postgraduate School AUVII 


the Ada language which produces object-oriented extensions such as inheritance and 
dynamic binding. 


2. Simulation Environment 

Simulation testing is performed on the software in the laboratory before the 
software is placed in the actual vehicle. Testing of the model in the laboratory was 
accomplished by linking three separate processors through an Ethernet connection using 
stream socket communications. The Strategic level was programmed in Prolog and CLIPS 
and ran on a Sun SPARCstation 4/280 using the UNIX operating system. The Tactical level 
was written in Classic-Ada and was also hosted on a Sun SPARCstation 4/280 running 
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UNIX. The Execution level and the simulator itself were programmed in C and ran on a 
Silicon Graphics 4D/340VGX workstation using the IRIX operating system. The three- 
processor test configuration is shown in Figure 2. 



Strategic Tactical Execution 

Level Level Level 


Figure 2 Original AUV Simulator Test Configuration 

This Classic-Ada implementation of the Tactical level is truly object-oriented in 
the sense that it allows inheritance of object characteristics and provides dynamic binding 
of operations to objects. However, this version employs a sequential approach to carry out 
required behaviors which presents some problems for multiple modes of operations. This 
thesis research is an extension of that work in an attempt to add Ada tasking for concurrent 









operations on the Mission Control Computer to fulfill the intent of RBM. The new Tactical 
level implementation relies on the Ada RTS without modification for task scheduling and 
is discussed in Chapter V. 
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III. THE RATIONAL BEHAVIOR MODEL 


A. INTRODUCTION 

The Rational Behavior Model (RBM) is an autonomous vehicle control software 
architecture composed of three distinct levels. The levels of RBM are based on the degree 
of abstraction of the problem domain, and they are, from highest to lowest: the Strategic, 
Tactical, and Execution levels [Kwak92]. The structure of RBM is illustrated in Figure 3. 


Programming 

Paradigm 

Logic 


it 

Strategic 

Level 


Object-oriented 



Tactical Level 


Imperative Execution Level 


Level of 
Abstraction 


Figure 3 RBM Structure 
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The power of RBM for software engineering lies in its tailoring available design 
resources to address the important aspects of the problem at hand. When the programming 
paradigm matches the abstraction of the problem instead of being forced into it, the result 
is robust and easily understood software. Such software can be modified with little 
difficulty, satisfying one of the key objectives of software engineering. 

B. STRATEGIC LEVEL 

The Strategic level stands at the top of the RBM hierarchy. At this level, the essence 
of a mission is expressed using clear, high-level logic so that the vehicle can act in a rational 
manner. Logic for sequencing behaviors is encapsulated at this top level. Simplicity is 
maintained by the Strategic level having no internal memory and no knowledge of 
operational details. Required mission behaviors are provided by the process of goal-driven 
decomposition. A root or mission goal is repeatedly refined into its constituent subgoals 
until primitive goals are reached. Implementation is initiated at this point. Because the 
reasoning process proceeds according to a deliberate sequence, the Strategic level can be 
expressed quite naturally in a rule-based programming language like Prolog or CLIPS. The 
rule set of the Strategic level is divided into mission specification and doctrine. The mission 
specification part deals with knowledge unique to a mission, while the doctrine part 
concerns mission-independent knowledge that is usually tied to the nature of the vehicle. 

Once a primitive goal is identified, the Strategic level calls on the Tactical level to 
start some type of appropriate behavior. These calls can be either queries or commands. 
Queries are information requests which require a binary response. Commands are orders 
requiring no feedback other than an acknowledgment of completion of the ordered task. If 
more information is needed to make a decision after a command has been issued, queries 
are used to poll the Tactical level [Bym93j. 

C. EXECUTION LEVEL 

The Execution level lies at the other end of the RBM hierarchy. It is responsible for 
the multitude of complex physical actions that comprise the primitive goals of the Strategic 
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level; therefore, it must guarantee basic vehicle stability. Stability is provided by a series of 
autopilots driven by servo loops. In addition, processes with hard real-time scheduling 
constraints are encapsulated at the Execution level. While computation at the Strategic 
level is purely symbolic, computation at the Execution level is completely numeric to 
ensure timing requirements are met. Implementation of this level requires an imperative 
programming language with good numeric computation speed such as C or Fortran. 

Since it is the base of the RBM hierarchy, the Execution level must act as the 
intermediary between the software and the hardware. This level receives setpoints and 
vehicle mode information from the Tactical level, and its autopilots must use these data 
repeatedly until they are updated. Autopilot commands are sent to motors, control surfaces, 
and other hardware devices using digital and analog signals. Information is received from 
analog hardware devices in the form of digital readings. Changes in hardware are mostly 
contained within the Execution level unless new tasks or new hardware capabilities are 
added. In this case, the Tactical level must be modified as well [Bym93]. 

D. TACTICAL LEVEL 

The Tactical level is the middle level in the tri-level RBM hierarchy and is the focus 
of this research. This level is the crucial link between the knowledge-based orientation of 
the Strategic level and the numeric-based orientation of the Execution level. Therefore, the 
primary objective of the Tactical level is to act as a bridge between the two end levels and 
cannot be discussed without reference to these two levels. This level responds to queries 
and commands from the Strategic level and inputs from the Execution level through 
specific behaviors. 

In its role as coordinator between the Strategic level and the Execution level, the 
Tactical level must be an analyst and translator. Abstract behaviors from the Strategic level 
must be analyzed and then translated into their executable details to be performed by the 
Execution level. The Tactical level takes the general descriptions of what the vehicle is 
supposed to do and supplements these with timing details and physical constraints of the 
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vehicle as it decomposes them into simpler and simpler behaviors. The resulting primitive 
behaviors, which consist of data requests and setpoint and control mode commands, are 
sent to the Execution level to be carried out [Kwak93]. 

Tactical level behaviors can be grouped under the entities which perform them. These 
entities have state, behavior, and identity and are called software objects [Booc91]. 
Objects, in turn, are organized into a hierarchy such that each parent object decomposes 
into one or more dependent, or child, objects. The object at the top of the hierarchy acts as 
the interface between the detail-free Strategic level and the rest of the hierarchy. An object 
at the Tactical level only has knowledge of its parent and its children and nothing else. To 
access any other object, including its own siblings, an object must go through the parent of 
that other object. The only exception to this rule is that data required by multiple objects 
can be retrieved directly from specifically designated database manager objects [Bym93]. 
Modifications and additions to the object hierarchy are facilitated by this structure. In 
addition, parallel threads of control can be identified among objects under different parents 
for concurrent execution [Kwak93]. 

E. TACTICAL LEVEL REQUIREMENTS 

Just as the quality of a bridge depends on its keystone, the strength of the Tactical level 
as an interface between the Strategic and Execution levels in RBM depends on its design 
specification. An appropriate structure for the design specification of the Tactical level is a 
basic requirement for implementation. The design pattern used for this research was the 
watch crew of a submarine, which provides a representative, well-understood model for 
Tactical level relationships [Bym93]. 

The design specification is not very useful unless it is supported by appropriate 
programming facilities. A programming language is the raw material out of which the 
Tactical level bridge is built. Its utility as a bridge depends on the appropriateness and 
power of the language chosen for implementation. The least that is required to represent the 
relationships of this level is an object-based language, although an object-oriented 
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language is preferred to accommodate future modification and growth. Some method for 
implementing concurrency is also necessary. Choosing a programming language is 
discussed in the next chapter. 
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IV. TACTICAL LEVEL PROGRAMMING LANGUAGES 


A. BACKGROUND 

There are numerous programming languages that are object-oriented or object-based. 
This number is reduced substantially when the criterion of constructs to support 
concurrency is considered. Many powerful object-oriented languages such as C++ and 
CLOS do not presently provide explicit support for concurrency. The remaining subset of 
languages is limited to Ada and its variants. The applicability of these languages to the 
Tactical level problem domain is now examined. 

B. ADA 

Ada is an object-based language developed for the United States Department of 
Defense to handle very large, software-intensive systems. Ada has numerous features 
which support object-oriented design, including packages, tasks, and generic units 
[Booc91], Since Ada has objects but does not have explicit classes, however, it has no 
mechanism for inheritance, dynamic binding, or polymorphism in its present form. 
Therefore, message passing between objects is detailed, complicating design in a large 
software system incorporating many related classes of objects. This does not pose a 
problem for the Tactical level as it is currently designed for the AUV, because an object 
hierarchy is sufficient to specify relationships. Future growth and redesign would be better 
accommodated by a class-based language. 

Concurrency is supported in Ada through its task construct. Tasks are based on the 
Communicating Sequential Processes (CSP) model [Hoar78] in which processes 
synchronize and then pass messages through input and output statements. This 
synchronization is called a rendezvous and is required between two processes before 
communication can occur. If one task reaches the rendezvous point before the other, it must 
wait or accept another task that is ready to pass a message. Exclusive access to data or a 
resource is thus built in with the CSP model, since a task can only communicate with one 

14 


1 


other task at any given time. Ada’s accept statements and entry calls function in the same 
way as CSP’s input and output statements, respectively, with some added features. First, 
communication in Ada tasks is bidirectional, while it is strictly unidirectional in CSP tasks. 
Second, to CSP’s parameter copying, the Ada rendezvous adds the capability for the called 
task to execute statements and return results to the calling task [Geha84], Although tasks 
cannot stand alone, they can be encapsulated as objects, providing a powerful abstraction 
mechanism for object-based applications that are concurrent in nature. Task objects are an 
excellent representation for the objects of the Tactical level which must perform multiple 
functions. 

C. CLASSIC-ADA 

Classic-Ada is a preprocessor for Ada which adds capabilities needed to complete the 
object-oriented paradigm. Processing Classic-Ada code yields pure Ada source code with 
special data structures to support inheritance, dynamic binding, and polymorphism. Data 
and behaviors for an object are written as instance variables and instance methods, 
respectively. These characteristics are unique to that object and its class. An object 
communicates with another object simply by using a send statement with the object name 
and the instance method name [Soft92]. This extension to Ada provides a much more 
concise method for message passing between objects. Messages can be passed without any 
bulky or artificial syntax as in Ada. Also, a class structure can be built which facilitates 
modifications to the Tactical level because of the built-in inheritance mechanism. 

Concurrency is supported in Classic-Ada through the Ada task construct. However, 
there is no provision for implementing tasks at the object level. Tasks can only be declared 
within methods, severing the link between objects and tasks that is available in Ada. This 
restriction severely limits the usefulness of Classic-Ada for implementing object-oriented 
designs that involve a significant amount of concurrency, such as the Tactical level. 
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D. ADA 9X 


Ada 9X is a revised version of Ada which updates the 1983 ANSI Ada standard. 
Although it is not yet commercially available, Ada 9X deserves examination. It will soon 
become the standard for Ada, and it incorporates some object-oriented capabilities. Ada 9X 
provides for inheritance, dynamic binding, and polymorphism through its tagged type 
construct, which allows components to be added to a type when it is derived. Public and 
private record types are the only types that can be tagged. 

Ada 9X also enhances the basic task construct for concurrent programming. More 
flexibility is provided in choosing priority and scheduling rules, task delay times can be 
made explicit, and asynchronous transfer of control is provided by additions to the task 
select statement [DoD93]. Nevertheless, the object-oriented paradigm is not extended to 

task types; task types cannot be tagged and thus are static in nature 1 . Since its task type is 
unchanged from Ada, Ada 9X offers no significant advantage for representing the 
concurrency of the Tactical level. 

E. COMPARISON OF PROGRAMMING LANGUAGES 

Ada, Classic-Ada, and Ada 9X all have advantages and disadvantages for the Tactical 
level application. Ada supports concurrency well with its rendezvous, providing a high- 
level model of communication to enforce mutual exclusion. Classic-Ada extends Ada but 
superimposes object-oriented features at a higher level rather than integrating them with 
Ada [Atki91]. The lack of object-level tasking is a serious drawback. Ada 9X offers 
promise for integrating object-oriented features with Ada in many areas but not in the area 
of concurrency. What is needed is a language that combines object-oriented and concurrent 
concepts, considering classes, objects, and tasks together. Figure 4 illustrates the current 
programming language situation. In the absence of such a language, Ada was chosen for its 
availability and the flexibility of its task construct. 


1. In Ada 9X. as in Ada, the number of tasks of a task type can be dynamic. 
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Figure 4 Tactical Level Programming Languages. 
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V. TACTICAL LEVEL IMPLEMENTATION 

A. OVERVIEW 

The quality of the Tactical level implementation depends significantly on the quality 
of its design. As mentioned in Chapter HI, the watch crew of a manned submarine offers a 
natural model for representing the entities and behaviors of the Tactical level. Using this 
model, an object hierarchy can be built which supports an implementation model. The 
implementation model is the method of construction of the Tactical level bridge; it 
determines how the raw material of the programming language gets put together on the 
keystone of the design model. 

B. DESIGN MODEL 

The design specification for the Tactical level is given in Figure 5. The blocks in the 
diagram stand for distinct entities within the Tactical level structure, and each one 
corresponds to a software object. The hierarchical structure of the Tactical level 
encompasses most of the objects and is indic ated by the dotted lines between them. The 
AUV Officer of the Deck (OOD) provides overall operational control at this level and 
stands at the top of the hierarchy. The OOD also provides the sole interface between the 
Strategic and Tactical levels. Top level primitive goals are handed to the OOD so that he 
can activate the behaviors understood by the Tactical level to satisfy those goals. In the 
watch crew, the Captain gives commands or requests the status of the submarine’s systems 
from the OOD. The OOD, in turn, in gives the required orders to satisfy the goal or answer 
the query issued by the Captain. 

The Tactical level objects cover all the behaviors that the vehicle can perform. 
Coordinating the operations of each object, the OOD insures each task is completed 
appropriately. Behaviors are implemented as methods within an object. For the most part, 
behaviors require the involvement of multiple objects. Communication between objects is 
accomplished through message passing. As mentioned, communication is limited to 
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parent-child pairs. In this scheme, efficiency is sacrificed to gain modularity of code and 
ease of understanding for the user. 

Just as all Strategic level communications must go through the conduit of the OOD, 
all contact with the Execution level is similarly constrained. Command packets comprised 
of setpoints and modes are transferred solely through the Command Sender object under 
the direction of the OOD. In addition, telemetry data is accepted from the Execution level 
by the Sensory Receiver object exclusively. The limitations on these interfaces eliminate 
command and data discrepancies. 

There are a number of objects that are disconnected from the object hierarchy in the 
Tactical level. These correspond to databases that serve any other requesting object any 
time their respective data are needed. They contain the state of the mission (Mission 
Model), the perceived state of the environment (World Model), recorded mission history 
(Data Recorder), and current sensor readings (Sensory Receiver) [Byrn93]. 

C. IMPLEMENTATION MODEL 

The implementation model gives life to the relationships expressed in the design 
model. The structure of the implementation model using Ada is illustrated in Figure 6. The 
methodology for this design was to provide concurrency between objects while adhering to 
the control requirements of RBM. Getting the AUV to execute a mission involving multiple 
modes of operation and showing that it can replan a mission in progress without giving up 
control were the goals of the implementation. The code for the implementation in Ada is 
found in Appendix A. 

1. Description of Communication 

Commands and queries are passed between Tactical level objects by means of 
task entry calls with boolean flags. Each command issued to the OOD has a goalflag which 
gets set to true when execution of the command is complete. A command is attempted until 
the goal flag is set to true to insure that it gets executed. Each query has a return flag and a 
goal flag. The return flag gets set to true when the appropriate object has received the 






























query. In this case, the goal flag gets set based on a positive or negative response to the 
query. A query is attempted until the return flag is set to true to insure that the query has 
been communicated to the target object. 

All upper level objects in the hierarchy are represented as tasks in Ada. Each of 
these tasks consists of a set of accept statements, which are messages for behaviors that the 
respective object or its children perform. Each accept statement further contains entry calls 
to child objects, and this chain of message passing continues until an object is reached that 
can execute part or all of a given command or answer a given query. An example of the 


message passing pattern is shown in Figure 7. 

....................... 

task A is j 

accept QUERY_A(GOAL_FLAG, RETURN_FLAG : out BOOLEAN) do 
if QUERY_A =TRUE then 
GOAL_FLAG := TRUE; 
else 

GOAL.FLAG := FALSE; 
end if; 

RETURN.FLAG := TRUE; 
end QUERY.A; 


accept COMMAND.A(GOAL_FLAG : out BOOLEAN) do 
task A_l.COMMAND_A(GOAL_FLAG_l): 
if GOAL_FLAG_l = TRUE then 

GOAL.FLAG := TRUE; 
else 

GOAL.FLAG := FALSE; 
end if; 

end COMMAND.A; 
end task A 


task A_1 is 

». accept COMMAND.A(GOAL_FLAG_l: out BOOLEAN) do 
do COMMAND.A; 

GOAL FLAG.l := TRUE; 
end COMMAND.A; 
end task A„1; 


Figure 7 Example of Task Communication 
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The lowest level objects are represented as procedures or functions, since these 
objects consist of only basic operations. As leaves on the object hierarchy tree, these objects 
require no further communication with any objects so implementing them as tasks would 
introduce unnecessary overhead. However, these objects must still be able to communicate 
with their parent objects while performing their respective functions to support RBM’s 
control scheme. Since the parent object task is suspended while it waits for the child to 
complete its required behavior, some alternate way must be used to pass messages to the 
parent during this time. 

The method of alternate communication used in this research was a series of 

router, or relay 1 , tasks. A relay task waits until it is called by a task with data to send and 
then immediately calls the next task in the series. This process continues until the data is 
consumed. Use of these intermediary tasks allows for a loosely coupled implementation, 
but this advantage must be balanced against the overhead of added tasks [Lema89] 
[Niel88], Relay tasks allow time-consuming behaviors such as search and homing to 
continue while the primary route of communication is suspended awaiting an answer to 
send back to the Strategic level. The situation is illustrated in Figure 8 using homing as an 
example. 

The database objects are also all implemented as tasks to insure only one object 
at a time can access any one of them. Otherwise, Sonar Control, for example, could set the 
vehicle’s mission mode in the Mission Model while the OOD is attempting to read that 
value. The Ada rendezvous enforces mutual exclusion, preventing such data 
inconsistencies. Only the first entry call is allowed to participate in the rendezvous. All 
others are queued and serviced sequentially. 


1 .Relay tasks are one of three types of intermediary tasks. Buffer tasks, which have an entry to 
accept data from a producer and an entry to send data to a consumer when requested, and trans¬ 
porter tasks, which request data using an entry call to a producer task and then provide the data to a 
consumer through an entry call, are the other types of intermediary tasks. 
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Figure 8 Router Task Communication 
2. Description of Objects 


a. OOD 

This object consists of two tasks, one for the main OOD functions and one for 
routing. As the top level of the object hierarchy, the main OOD task must contain accept 
statements for all of the primitive goals issued by the Strategic level. Entry calls within each 
accept statement activate the behaviors necessary to satisfy a particular goal. The main 
OOD task must also coordinate these behaviors. The OOD relay task acts as a backup 
channel to the Command Sender when the main OOD task is suspended waiting for a 
command to be executed. 


b. Navigator 

This object also contains a main task and a routing task. The main Navigator 
task is responsible for guidance, position estimation, and path replanning. This task’s view 
of the world at any given time extends only from its present position to the next waypoint 
to make its operation as generic as possible. All mission details are encapsulated in the 
Mission Model. Following the OOD’s model, the main Navigator task passes on orders to 
its subordinates using entry calls and coordinates their actions. In the case of mission 
replanning, this coordination involves concurrency, as guidance for loitering must be 
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provided at the same time as the mission route is being replanned. The Navigator relay task 
acts as a backup channel to the OOD when the main Navigator task is suspended waiting 
for a command to be executed. 


c. Guidance 

This object is comprised of a main task and a routing task as well. The 
responsibility of the main Guidance task is to provide the heading and depth setpoints to be 
included in the command packet sent to the Execution level. The accept statements in this 
task contain calls to procedures that do various types of guidance. 

For this study, line-of-sight (LOS) guidance and homing guidance were both 
implemented. The new command heading to a waypoint is computed for LOS guidance as 


follows: 


^cmd = atan 


(Y - Y ) 

v * next 1 curr 1 
next ~ ^curr ) 


(Eql) 


where: 


X curn Y curr = X, Y components of AUV’s current position. 

X nexl , Y nexl = X, Y components of next waypoint. 

The new command heading to a target is computed for homing guidance using 
the following equation: 


¥ . = ¥ + B 

cmd curr ~ 


(Eq2) 


where: 


curr - Current vehicle heading. 

P = Sonar relative bearing to target. 

The Guidance relay task acts as a backup channel to the Navigator when the 
main Guidance task is suspended waiting for a command to be executed. 
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d. GPS Control 

This object is responsible for controlling the Global Positioning System 
receiver and accessing it for navigation. This capability was not modeled for this research. 
The GPS Control task in this implementation simply returns a positive response when a 
GPS fix is requested. Research on integrating GPS in this environment is included in 
[Stev93]. 

e. Sonar Control 

This object issues sonar commands, checks for and logs objects, and monitors 
the sonar for various tasks such as search. In this study, this object consists of a single task 
which monitors the sonar range and bearing values while the vehicle executes the command 
“do search pattern”. The task executes an expanding box search algorithm until threshold 
values for both range and bearing are detected from the sonar. The search pattern and 
algorithm are shown in Figure 9. 

f Dead Reckoning 

This object provides present position based on a known position fix, actual 
heading, and elapsed time. The Tactical level dead reckoner serves as a backup to the 
Execution level dead reckoner to crosscheck its operation. The dead reckoner was not 
implemented for this study. 

g. Mission Replanner 

This object has a single task to perform local replanning for avoiding 
obstacles and global replanning to accommodate a vehicle fault. Global replanning was 
modeled by using a delay statement and instantaneously changing the mission route 
through the Mission Model. 

h. Engineer 

This object consists of one task to monitor the condition of each vehicle 
system. For this study, a thruster system problem was modeled by reducing the thrust level 
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B ^2 B1 



Bi* indicates the 
position in the 
search pattern 
where: 

i = Box number 
k = Leg number 


Algorithm DO_SEARCH_PATTERN 
begin 

NEXT TIME := CLOCK + INTERVAL - TURN.TIME; 

LEG.NUM := 0; 

Initialize SEARCH_HEADING 
loop 

if CLOCK > NEXT_TIME then -Change heading for new leg 
if LEG_NUM = 2 then -Expand the box 
LEG.TIME := LEG_TIME + INTERVAL; 

LEG_NUM := 1; 
end if; 

-'Change heading to make box comer and normalize 
if SEARCH_HEADING > (PI / 2) then -Command heading > 0 
SEARCH.HEADING := SEARCH.HEADING - (PI / 2); 
else -Command heading <= 0 
SEARCH.HEADING := SEARCH.HEADING + (3 PI / 2); 
end if; 

LEG.NUM := LEG.NUM + 1; 

NEXT.TIME := NEXT.TIME + LEG.TIME; 
end if; 

Receive SONAR.BEARING and SONAR RANGE 
Send SEARCH.HEADING and SEARCH.MODE 

exit when SONAR.RANGE < RNG.LIMIT and ABS(SONAR.BEARING) < BRG.LIMIT; 
end loop; 

end DO.SEARCH.PATTERN; 


Figure 9 Expanding Box Search Pattern and Algorithm 
gradually from an initial value until it moved below a given threshold. Accept statements 
for all other system checks give a negative response to indicate the systems are operating 
properly. 


L Weapons Officer 

The Weapons Officer is comprised of one task that is responsible for 
monitoring and delivering the vehicle’s payload. This capability was not implemented for 
this research. The command to employ weapons simply returns a positive response. 
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j. Command Sender 

This object accepts command packets built by the OOD and sends them to the 
Execution level. A command packet consists of command X and Y coordinates, command 
heading, command depth, command speed, and mode. Since this object just relays data and 
cannot be accessed by any object other than the OOD, it was implemented as a procedure. 
The physical separation of the Tactical and Execution levels in this study required 
additional procedures for network communications. 

Jt Sensory Receiver 

This object consists of a single task that accepts telemetry records from the 
Execution level, stores the individual values, and provides the data to other Tactical level 
objects when requested. Each sensory packet contains vehicle position represented as X 
and Y coordinates, altitude above the bottom, and depth. This object is also responsible for 
putting a time stamp on a sensory packet before sending it to the Data Recorder, although 
this feature was not implemented in this work. 

L Mission Model 

This object is comprised of one task to hold and manage the waypoints that 
make up the mission route and the vehicle modes for the various phases of the mission. For 
the purposes of this thesis, these values were entered in data files which were read in by the 
Mission Model upon initialization of the simulator. 

m. World Model 

This object has one task to hold and manage known objects and other 
environmental data. Obstacles were the only type of environmental data used in this study. 
These data were entered in files and read in during initialization as the Mission Model data 
was. 
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n. Data Recorder 

This object consists of a single task to accept and maintain telemetry records 
and other explanatory messages for post-mission analysis. This object was not modeled for 
this research. 

3. Mission Environment 

A mission in reality involves multiple phases and the possibility of unforeseen 
system problems. Such an environment requires the AUV to operate in more than one mode 
and the OOD to coordinate the behaviors of Tactical level objects concurrently as well as 
sequentially. 

The target mission for this research was a search-and-rescue mission developed 
by the 1992 National Science Foundation workshop on furthering and evaluating autonomy 
in the area of underwater vehicle technology [Stee92]. In this mission, the AUV must 
traverse a given search area, locate a subsurface buoy, cut the buoy’s mooring line, drop a 
package as close to the buoy as possible, return to the launch site, and surface. The 
interpreted rule set for this mission written in Prolog is presented in [Byrn93]. The mission 
is broken down into the following four phases: transit, search, task, and return. 

The vehicle has four modes that correspond directly to the four mission phases. 
Transit and return are basically the same at the Tactical level. Navigation is executed using 
LOS guidance after the Navigator receives each query about whether a waypoint is reached. 
The only concurrency implemented in these modes is this execution of LOS guidance as 
the Tactical level releases control back to the Strategic level for the next command to be 
issued, and this is minimal. 

Initiation of the search mode creates problems for a sequential implementation. 
The Strategic level must know the search is completed before issuing the next command, 
and so it waits on the OOD. The OOD waits on the Navigator, which waits on Sonar 
Control. While all these tasks are suspended, control of the vehicle must be maintained for 
the search through the objects that are waiting for the search to complete. Therefore, a 
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series of relay tasks is required in Ada to provide intra-object concurrency. The situation is 
the same in the task mode while homing is being performed. The OOD waits on the 
Navigator, which waits on Guidance, which waits on the Homing Calculator. The sequence 
of router tasks allows homing guidance commands to get through while these other tasks 
await the completion of homing. 

When a system problem occurs, multitasking is required to maintain control of the 
vehicle during route replanning. The Strategic level issues the command to start replanning 
to the Tactical level when a system problem is encountered. The Navigator must send a 
command to the Mission Replanner to start replanning simultaneously with a command to 
Guidance to loiter. In Ada, this is accomplished by first issuing a parameterless entry call 
to the Mission Replanner, which has a simple accept call and a separate set of statements 
to perform replanning. This entry call is followed by an entry call to Guidance to loiter, and 
the Navigator task is suspended until loitering is done. Suspension of the Navigator task 
requires Guidance to utilize the router tasks to send commands to the Execution level as in 
the case of the search and task modes. The replanning operation and loitering guidance 
continue in parallel until replanning is done with the Ada RTS providing the scheduling of 
the two tasks. The situation is illustrated in Figure 10. Thus, inter-object concurrency is 
provided in addition to the intra-object concurrency provided by the relay tasks. 

Operation of the implementation in a mission -oriented environment is discussed 
in the next chapter. 
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Figure 10 Multitasking in Route Replanning 
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VI. TESTING AND RESULTS 


A. INTRODUCTION 

Testing the Tactical level implementation was accomplished using the simulation 
facilities available in the laboratory. The simulation environment was set up to reflect the 
actual hardware and software configuration on the NPS AUV. Mission scenarios were then 
developed to represent the conditions of the search-and-rescue mission described in 
Chapter V. The AUV graphical simulator provided for the entry of waypoints and obstacles 
using Cartesian coordinates in a visual model of the NPS pool to support this scenario 
development [Ong90]. 

B. SIMULATION ENVIRONMENT 

To test the implementation, modifications were made to the configuration described in 
Chapter II to reproduce the environment on the vehicle. Two processors were used to 
represent the two processors on the actual vehicle. The Strategic and Tactical levels were 
run together under the UNIX operating system on a Sun SPARCstation 3/180, 
corresponding to the Mission Control Computer. The Strategic level was coded in CLIPS- 
Ada, a preprocessor which compiles CLIPS code to Ada source code, to allow the Strategic 
and Tactical levels to reside on the same processor. A description of this CLIPS-Ada 
implementation and the code are presented in [Scho93]. The Tactical level was coded in 
Ada, as described in Chapter V. The Execution level used the same C code as the previous 
implementation and was again run under the IRIX operating system on a Silicon Graphics 
4D/340VGX Workstation, corresponding to the Vehicle Control Computer. The two- 
processor test configuration is shown in Figure 11. 

A sonar model was required for the simulation so that all phases of the mission could 
be tested. Sonar was simulated by adding code to the Sensory Receiver to track range and 
bearing to a target, which was represented by an obstacle entered into the World Model. 
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Figure 11 AUV Simulator Test Configuration and Vehicle Configuration 

This modification allowed the search and task modes of the AUV to be demonstrated 
realistically. 

A vehicle mode was entered along with each waypoint in the waypoint data file that 
the simulator read into the Mission Model. In this way, a vehicle mode could be selected at 
each waypoint based on the mission profile. Available choices for the vehicle mode include 

transit, search, and return *. 


1. Task is an invalid choice because this mode is automatically triggered by the successful comple¬ 
tion of the search mode. When the search ends, homing begins, initialing the task mode. 
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C. SCENARIOS 


1. Multi-Phase Mission 


The first scenario tested was the straight four-phase search-and-rescue mission. 
For this scenario, a set of three waypoints and a single obstacle were chosen to cover the 
four mission phases. Figure 12 depicts the mission route. The vehicle was programmed for 



the transit mode during the first leg, corresponding to the transit phase of the mission. The 
vehicle simply executes LOS guidance between waypoints in this mode. At the first 
waypoint, the vehicle was programmed to change to the search mode and execute an 
expanding box search pattern, corresponding to the search phase of the mission. The 
vehicle was then set to transition automatically to its task mode, corresponding to the task 
phase of the mission. The vehicle executes homing guidance in this mode with the obstacle 
as its target. The vehicle completes the task upon reaching its target. After reaching the 
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target, the vehicle was programmed to change to the return mode for the last two legs, 
corresponding to the return phase of the mission. 

2. Multi-Phase Mission With Route Replanning 

This scenario used the same mission route and vehicle modes as the first one. A 
low thrust level, simulating a thruster system problem, was programmed to occur during 
the transit phase. When faced with such a problem, the vehicle simultaneously loiters and 
shortens its mission route to insure it reaches its final goal before system degradation 
becomes too serious. Route replanning is accomplished in this implementation by sending 
a message to the Mission Model requesting a shortened route. In reality, the Mission 
Replanner would d ermine this shortened route and pass the modified waypoint data to the 
Mission Model in the message. The vehicle was programmed in this run to eliminate the 
search and task phases of the mission and to go straight to its return mode for the mission’s 
return phase. 

D. RESULTS 

In the first scenario, the vehicle successfully executed all phases of the mission, 
transitioning through all its modes and reaching all waypoints and the target. There was a 
problem w th communication between the Tactical and Execution levels due to the 

simulator protocol 2 . This problem arose because of the combination of the long line of 
communication to the Command Sender and the short line of communication to the 
Sensory Receiver under RBM. The problem was averted by using a short delay during the 
search and task modes. 

In the second scenario, the vehicle accomplished both of its simultaneous tasks. It 
loitered in place after detecting the system problem for the time of the programmed delay. 


2. The simulator requires an even balance between transmissions and receptions. Whenever it sends 
a set of data, it must receive a command packet before it can send another set. The actual vehicle is 
not subject to this constraint. 
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proceeded to the first waypoint, transitioned to the return mode, and completed the return 
phase of the mission. 

Traces of the execution of the Tactical level code under these two mission scenariosare 
found in Appendix B. A user’s guide for the AUV simulator is provided in Appendix C. 




VII. CONCLUSIONS AND FUTURE WORK 


In this thesis, a concurrent, object-based implementation is developed and evaluated 
for the Tactical level of the Rational Behavior Model. Previous work in this area has 
focused on object-oriented implementation exclusively or minimal use of concurrent 
programming facilities. However, the Tactical level is the essential bridge between the top 
and bottom levels of RBM, and it must handle concurrent, as well as sequential, operations 
among its objects for the success of the model in practice. In the absence of a programming 
language that combines object-oriented features with constructs for concurrency, Ada 
remains the best choice for an implementation of the Tactical level. The Tactical level 
implementation in this work uses relay tasks for intra-object concurrency to handle 
multiple phases of a mission and parameterless task entry calls for inter-object concurrency 
to handle route replanning. Both of these mechanisms insure control of the vehicle is 
maintained throughout a mission. Simulation testing shows that control of the vehicle is 
indeed maintained continuously with such an implementation even in the face of time- 
consuming tasks. 

A. RESEARCH CONTRIBUTIONS 

This research has numerous benefits. First, it provides an example for implementing 
multitasking to aid in the control of autonomous vehicles. This capability is very important 
for them to reflect rational behavior. Second, this work reiterates the value of the object- 
oriented paradigm for this problem domain. Object-oriented techniques increase the 
modularity and simplicity of the Tactical level implementation, improving the reliability 
and maintainability of the software. Finally, this research reveals the weakness of current 
programming languages in integrating concurrency with the object-oriented paradigm. 

B. SUGGESTIONS FOR FUTURE RESEARCH 

There are many ways to build on the foundation this research has established. One area 
that was started in this work but not completed was transferring the simulator 
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implementation to the actual vehicle and testing it. Another area for future research is 
developing a more complete implementation and testing how much load one processor can 
bear. Extensive use of Ada tasks, especially such intermediary tasks as relay tasks, imposes 
a significant amount of overhead, and time did not permit a full analysis of this factor in 
this work. Finally, distributed implementations of the Tactical level represent fertile ground 
for future work, since the NPS AUV is fitted with a transputer board. Progress in any of 
these areas would make the Tactical level a stronger, more robust link in RBM. 
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APPENDIX A. TACTICAL LEVEL SOURCE CODE 


--Title : tacJv_s.a 

-Author : FJ\ Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specifications for procedures for Ada side of Clips-Ada/Ada 
interface for simulator version of AUV Tactical level 


package TACTICAL_LE VEL1 is 

procedure READY_VEHlCLE_FOR_LAUNCH(GOAL_FLAG : in out INTEGER); 
procedure SELECT_FIRST_WAYPOINT(GOAL_FLAG : in out INTEGER); 
procedure ALERT_USER(GOAL_FLAG; in out INTEGER); 
procedure IN_TRANSIT_P(GOAL_FLAG ; in out INTEGER); 
procedure TRANSIT_DONE_P(GOAL_FLAG : in out INTEGER); 
procedure IN_SEARCH_P(GOAL_FLAG ; in out INTEGER); 
procedure SEARCH_DONE_P(GOAL_FLAG: in out INTEGER); 
procedure IN_TASK_P(GOAL_FLAG : in out INTEGER); 
procedure TASK_DONE_P(GOAL_FLAG : in out INTEGER); 
procedure IN_RETURN_P(GOAL_FLAG : in out INTEGER); 
procedure RETURN_DONE_P(GOAL_FLAG : in out INTEGER); 
procedure WAIT_FOR_RECOVERY(GOAL_FLAG: in out INTEGER); 
procedure SURFACE(GOAL_FLAG ; in out INTEGER); 
procedure DO_SEARCH_PATTERN(GOAL_FLAG: in out INTEGER); 
procedure HOMING(GOAL_FLAG : in out INTEGER); 
procedure DROP_PACKAGE(GOAL_FLAG: in out INTEGER); 
procedure GET_GPS_FIX(GOAL_FLAG : in out INTEGER); 
procedure GET_NEXT_WAYPOINT(GOAL_FLAG : in out INTEGER); 
procedure SEND_SETPOINTS_AND_MODES(GOAL_FLAG : in out INTEGER); 
procedure REACH_WAYPOINT_P(GOAL_FLAG : in out INTEGER): 
procedure GPS_NEEDED_P(GOAL_FLAG : in out INTEGER); 
procedure UNKNOWNJ3BSTACLE_P(GOAL_FLAG; in out INTEGER); 
procedure LOG_NEW_OBSTACLE(GOAL_FLAG : in out INTEGER); 
procedure LOITER(GOAL_FLAG : in out INTEGER); 
procedure START_LOCAL_REPLANNER(GOAL_FLAG : in out INTEGER); 
procedure START_GLOBAL_REPLANNER(GOAL_FLAG: in out INTEGER); 
procedure POWER_GONE_P(GOAL_FLAG : in out INTEGER); 
procedure COMPUTERJ5YSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure PROPULSION_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure STEERING_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure DIVING_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure BUOYANCY_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure THRUSTER_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER); 
procedure LEAK_TEST_P(GOAL_FLAG : in out INTEGER); 
procedure PAYLOAD_PROB_P(GOAL_FLAG : in out INTEGER); 
end TACTIC AL_LEVEL 1; 
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••Title : tacjv_ba 

-Author : FP. Thornton Jr. 

••Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Procedures for Ada side of CLIPS-Ada/Ada interface for 
simulator version of AUV tactical level 


with TEXT JO, OOD; 
use TEXT_IO,OOD; 

package body TACTICAL_LEVEL1 is 

package FLOATJNOUT is new FLOAT_IO(FLOAT); 
package INTEGERJNOUT is new INTEGER_IO( INTEGER); 
use FLOATJNOUT. INTEGER.INOUT; 

procedure READY_VEHICLE_FOR_LAUNCH(GOAL_FLAG : in out INTEGER) is 
begin 

THE.OOD.CREATE; 

THE_OOD.READY_VEHICLE_FOR_LAUNCH(GOAL_FLAG); 
PUT(“READY_VEHICLE_FOR_LAUNCH GOAL FLAG = “); 
PUT(GOAL_FLAG. WIDTH=>3); 

NEW_LINE; 

end READY_VEHICLE_FOR_LAUNCH; 

procedure SELECT_FIRST_WAYPOINT(GOAL_FLAG : in out INTEGER) is 
begin 

THE_OOD.SELECT_FIRST_WAYPOINT(GOAL_FLAG): 

PUT( “SELECT_FIRST_WAYPOINT GOAL FLAG = ”); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEWSLINE; 

end SELECT_FIRST_W AYPOINT; 

procedure ALERT_lJSER(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_pOD.ALERT_USER(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUTCALERTJJSER GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEWSLINE; 
end ALERT.USER; 

procedure IN_TRANSIT_P(GOAL_FLAG : in out INTEGER) is 
RETURN.FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.IN_TRANSIT_P(GOAL_FLAG. RETURN_FLAG); 
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exit when RETURN_FLAG = 1; 
end loop; 

PUT(“IN_TRANSIT_P GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW.LINE; 
end IN_TRAN S IT_P; 

procedure TRANSIT_DONE_P(GOAL_FLAG : in out INTEGER) is 
RETURNJFLAG: INTEGER := 0; 
begin 
loop 

THE_OOD.TRANSIT_DONE_P(GOAL_FLAG. RETURN.FLAG); 
exit when RETURN FLAG = 1; 
end loop; 

PUT(“TRANSIT_DONE_P GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 
end TRANSIT_1X)NE_P; 

procedure IN_SEARCH_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.IN_SEARCH_P(GOAL_FLAG, RETURN_FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“IN_SEARCH_P GOAL FLAG = "); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEWJLINE; 
end IN_SEARCH_P; 

procedure SEARCH_DONE_P(GOAL_FLAG : in out INTEGER) is 
RETURN JFLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.SEARCH_DONE_P(GOAL_FLAG, RETURN_FLAG): 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“SEARCH_DONE_P GOAL FLAG = "); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 
end S E ARC H_DONE_P; 

procedure IN_TASK_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG : INTEGER ;= 0; 
begin 
loop 

THE_OOD.IN_TASK_P(GOAL_FLAG, RETURN_FLAG); 
exit when RETURN.FLAG = 1; 
end loop; 

PUT(“IN_TASK_P GOAL FLAG = “); 
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PUT(GOAL_FLAG, WIDTH*>3); 

NEW.LINE; 
end IN.TASK.P; 

procedure T AS K_DONE_P(GO AL.FL AG : in out INTEGER) is 
RETURN.FLAG: INTEGER := 0; 
begin 
loop 

THE_OOD.TASK_DONE_P(GOAL_FLAG, RETURN.FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUTCTASKJDONE.P GOAL FLAG = "); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEWSLINE; 
end TASK_DONE_P; 

procedure IN_RETURN_P(GOAL_FLAG : in out INTEGER) is 
RETURN.FLAG ; INTEGER := 0; 
begin 
loop 

THE_OOD.IN_RETURN_P(GOAL_FLAG, RETURN.FLAG); 
exit when RETURN.FLAG * 1; 
end loop; 

PUTCTN.RETURN.P GOAL FLAG = “); 

PUT (GO AL.FL AG, WIDTH=>3); 

NEW.LINE; 
end IN.RETURN.P; 

procedure RETURN_DONE_P(GOAL_FLAG ; in out INTEGER) is 
RETURN.FLAG : INTEGER ;* 0; 
begin 
loop 

THE_OOD.RETURN_DONE_P(GOAL_FLAG, RETURN.FLAG); 
exit when RETURN.FLAG = 1; 
end loop; 

PUTC'RETURN.DONE.P GOAL FLAG = "); 

PUT(GO AL.FL AG, WIDTH=>3); 

NEW.LINE; 
end RETURN.DONE.P; 

procedure WATT.FOR.RECOVERY (GO AL.FL AG : in out INTEGER) is 
begin 
loop 

THE_OOD.WAIT_FOR_RECOVERY(GOAL.FLAG); 
exit when GOAL.FLAG = 1; 
end loop; 

PUTC'WAJT.FOR.RECOVERY GOAL FLAG = “); 

PUT(GO AL.FL AG, WIDTH=>3); 

NEW.LINE; 

end WATT.FOR.RECOVERY; 
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procedure SURFACE(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.SURFACE(GOAL_FLAG); 
exit when GOAL.FLAG = I; 
end loop; 

PUT(“ SURFACE GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEWJLINE; 
end SURFACE; 

procedure DO_SEARCH_PATTERN(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.DO_SEARCH_PATTERN(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT( ‘DO_SEARCH_PATTERN GOAL FLAG = “): 

PUT(GOAL_FL AG, WIDTH=>3); 

NEW_LINE; 

end DO_SEARCH_PATTERN; 

procedure HOMING(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.HOMING(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUTC'HOMING GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW_LINE; 
end HOMING; 

procedure DROP_PACKAGE(GOAL_FLAG: in out INTEGER) is 
begin 
loop 

THE_OOD.DROPJPACKAGE(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT(“DROP_PACKAGE GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW.LINE; 
end DROP_PACKAGE; 

procedure GET_GPS_FIX(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.GET_GPS_FIX(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT(*‘GET_GPS_FIX GOAL FLAG = “); 
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PUT(GOAL_FLAG, WIDTH->3): 

NEW.LINE; 
end GET_GPS_FIX; 

procedure GET_NEXT_WAYPOINT(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.GET_NEXT_WAYPOINT(GOAL_FLAG): 
exit when GOAL_FLAG = 1; 
end loop; 

PUT("GET_NEXT_WAYPOINT GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEWSLINE; 

end GET_NEXT_WAYPOINT; 

procedure SEND_SETPOINTS_AND_MODES(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.SEND_SETPOINTS_AND_MODES(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT("SEND_SETPOINTS_AND_MODES GOAL FLAG = -); 
PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 

end SEND_SETPOINTS_AND_MODES; 

procedure REACH_WAYPOINT_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG ; INTEGER := 0; 
begin 
loop 

THE_OOD.RE ACH_W A Y POINT_P(GO AL_FL AG, RETURN_FL AG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“REACH_WAYPOINT_P GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 

end REACH_WAYPOINT_P; 

procedure GPS_NEEDED_P(GOAL_FLAG : in out INTEGER) is 
RETURNJFLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.GPS_NEEDED_P(GO ALJFLAG, RETURN_FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“GPS_NEEDED_P GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW_LINE; 
end GPS_NEEDED_P; 

procedure UNKNOWN_OBSTACLE_P(GOAL_FL AG : in out INTEGER) is 
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RETURN.FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.UNKNO WN_OBST ACLE_P(GO AL_FL AG, RETURN_FL AG); 
exit when RETURN.FLAG = 1; 
end loop; 

PUT(“UNKNOWN_OBSTACLE_F GOAL FLAG = “); 

PUT(GOAL_FL AG, WIDTH=>3); 

NEW.LINE; 

end UNKNOWN_OB ST ACLE_P; 

procedure LOG_NEW_OBSTACLE(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.LOG_NEW_OBSTACLE(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT(“LOG_NEW_OBSTACLE GOAL FLAG = “); 

PUT (GO AL_FL AG, WIDTH=>3>; 

NEW_LINE; 

end LOG_NE W_OBST ACLE; 

procedure LOITER(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.LOITER(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT(“LOITER GOAL FLAG = "); 

PUT(GOAL_FLAG, WIDTHS); 

NEW_LINE; 
end LOITER; 

procedure START_LOCAL_REPLANNER(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.START_LOCAL_REPLANNER(GOAL_FLAG); 
exit when GOAL_FLAG = 1; 
end loop; 

PUT(“START_LOCAL_REPLANNER GOAL FLAG = “); 

PUT (GO AL_FL AG, WIDTH=>3); 

NEW.LENE; 

end START_LOCAL_REPLANNER; 

procedure START_GLOBAL_REPLANNER(GOAL_FLAG : in out INTEGER) is 
begin 
loop 

THE_OOD.ST ART_GLOB AL_REPL ANNER(GOAL_FLAG); 
exit when GOAL_FLAG - 1; 
end loop: 

PUT( 'START_GLOBAL_REPLANNER GOAL FLAG = “); 
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PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 

end START.GLOBAL_REPL ANNER; 

procedure POWER_GONE_P(GOAL FLAG : in out INTEGER) is 
RETURN.FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD J*0 WER_GONE_P(GO AL_FL AG, RETURN.FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(‘*POWER_GONE_P GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 
end POWER_GONE_P; 

procedure COMPUTER_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.COMPUTER_SYSTEM_PROB_P(GOAL_FLAG < RETURN_FLAG); 
exit when RETURN FLAG = 1; 
end loop; 

PUT(“COMPUTER_SYSTEM_PROB_P GOAL FLAG = “); 

PUT(GOAL_FLAG. WIDTH=>3); 

NEWSLINE; 

end COMPUTER_SYSTEM_PROB_P; 

procedure PROPULSION_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.PROPULSION_S Y STEM_PROB_P( GO AL_FL AG. RETURN_FL AG); 
exit when RETURN FLAG = 1; 
end loop; 

PUT(“PROPULSION_SYSTEM_PROB_P GOAL FLAG = “): 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW_LINE: 

end PROPULSION_SYSTEM_PROB_P; 

procedure STEERING_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG ; INTEGER := 0; 
begin 
loop 

THE_OOD.STEERING_SYSTEM_PROB_P(GOAL_FLAG, RETURN.FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“STEERING_SYSTEM_PROB_P GOAL FLAG = “); 

PUT(COAL_FLAG, WIDTH=>3); 

NEW.LINE; 

end STEERING_SYSTEM_PROB_P; 
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procedure DIVING_S YSTEM_PROB_P(GOAL_FLAG : in out INTEGER) is 
RETURN.FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.DIVING_SYSTEM_PROB_P(GOAL_FLAG, RETURN_FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“DIVING_SYSTEM_PROBLEM_P GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LINE; 

end DIVING_S YSTEM_PROB_P; 

procedure BUOYANCY_S YSTEM_PROB_P(GOAL_FL AG : in out INTEGER) is 
RETURN_FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.BUOYANCY_SYSTEM_PROB_P(GOAL.FLAG. RETURN.FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“BUOYANCY_SYSTEM_PROB_P GOAL FLAG = “); 

PUT(GOAL_FLAG, WIDTH=>3); 

NEW_LDME; 

end BUOY ANCY_S YSTEM_PROB_P; 

procedure THRUSTER_SYSTEM_PROB_P(GOAL_FLAG : in out INTEGER) is 
RETURN_FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.THRUSTER_SYSTEM_PROB_P(GOAL_FLAG. RETURN_FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“THRUSTER_SYSTEM_PROB_P GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW_LINE; 

end THRUSTER_SYSTEM_PROB_P; 

procedure LEAK_TEST_P(GOAL_FLAG : in out INTEGER) is 
RETURN.FLAG : INTEGER := 0; 
begin 
loop 

THE_OOD.LEAK_TEST_P(GOAL_FLAG, RETURN.FLAG); 
exit when RETURN_FLAG = 1; 
end loop; 

PUT(“LEAK_TEST_P GOAL FLAG = “); 

PUT(GOAL_FL AG, WIDTH=>3); 

NEW_LINE; 
end LEAK_TEST_P; 

procedure PAYLOAD_PROB_P(GOAL_FLAG: in out INTEGER) is 
RETURN.FLAG : INTEGER ;= 0; 
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begin 

loop 

THE_(X)DPAYLOAD_PROB_P(GOAL_FLAG, RETURN.FLAG): 
exit when RETURN_FLAG a 1; 
end loop; 

PUT(“PAYLOAD_PROB_P GOAL FLAG = “); 

PUT(GO AL_FL AG, WIDTH=>3); 

NEW.LINE; 

end PAYLOAD_PROB_P; 
end TACT1CAL_LE VEL1; 
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--Title : ood_s.a 

-Author : F.P. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Specification for OOD task 


package OOD is 
task THE_OOD is 
entry CREATE; 

* entry READY_VEH1CLE_F0R_LAUNCH(G_FLAG ; out INTEGER); 

entry SELECTJTRST_WAYPOINT(G_FLAG : out INTEGER); 
entry ALERT_USER(GJFLAG : out INTEGER); 
entry IN_TRANSIT_P(G_FLAG, R_FLAG ; out INTEGER); 
entry TRANSIT_DONE_P(G_FLAG, R.FLAG : out INTEGER); 
entry IN_SEARCH_P(G_FLAG, R_FLAG : out INTEGER); 
entry SEARCH_DONEJP(G_FLAG, R_FLAG : out INTEGER); 
entry IN_TASK_P(G_FLAG, R_FLAG : out INTEGER); 
entry TASK_DONE_P(GJFLAG, R_FLAG : out INTEGER); 
entry IN_RETURN_P(G_FLAG, R_FLAG : out INTEGER); 
entry RETURN_DONE_P(G_FLAG, R_FLAG : out INTEGER); 
entry WAIT_FOR_RECOVERY(G_FLAG : out INTEGER); 
entry SURFACE(G_FLAG ; out INTEGER); 
entry DO_SEARCH_PATTERN(G_FLAG : out INTEGER); 
entry HOMING(G_FLAG : out INTEGER); 
entry DROP_PACKAGE(G_FLAG : out INTEGER); 
entry GET_GPS_FIX(G_FLAG: out INTEGER); 
entry GET_NEXT_WAYPOINT(G_FLAG : out INTEGER); 
entry SEND_SETPOENTS_AND_MODES(G_FLAG : out INTEGER); 
entry REACH_WAYPOINT_P(G_FLAG, R.FLAG ; out INTEGER); 
entry GPS_NEEDED_P(G_FLAG, RJFLAG : out INTEGER); 
entry UNKNOWN_OBSTACLE_P(G_FLAG, R_FLAG ; out INTEGER); 
entry LOG_NEW_OBSTACLE(G_FLAG : out INTEGER); 
entry LOITER(G_FLAG : out INTEGER); 
entry START_LOCAL_REPLANNER(G_FLAG : out INTEGER); 
entry START_GLOBAL_REPLANNER(G_FLAG :out INTEGER); 
entry POWER_GONE_P(G_FLAG, R.FLAG : out INTEGER); 
entry COMPUTER_SYSTEM_PROB_P(G_FLAG, R_FLAG : out INTEGER); 
entry PROPULSION_SYSTEM_PROB_P(G_FLAG, R.FLAG: out INTEGER); 
entry STEERING JSYSTEMJ>ROB_P(G_FLAG, R.FLAG : out INTEGER); 
entry DIVING_SYSTEM_PROB_P(G_FLAG, R.FLAG : out INTEGER); 
entry BUOYANCY_SYSTEM_PROB_P(G_FLAG. R_FLAG : out INTEGER); 
entry THRUSTER_SYSTEM_PROB_P(G_FLAG, R_FLAG : out INTEGER); 
entry LEAK_TEST_P(G_FLAG, R_FLAG : out INTEGER); 
entry PAYLOAD_PROB_P(G_FLAG, R.FLAG : out INTEGER); 
end THE_OOD; 

end OOD; 
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-Title : ood_b-a (CLIPS-Ada Simulator Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Body for OOD task 


with TEXTJO, COMMAND_SENDER, MISSION.MODEL, WORLD.MODEL, 

SENSOR YJtECEIVER, 

OOD_ROUTER, NAVIGATOR. ENGINEERING. WEAPONS; 
use TEXTJO. MISSION.MODEL, WORLD.MODEL, SENSORY_RECEIVER,OOD_ROUTER. 
NAVIGATOR, ENGINEERING. WEAPONS; 

package body OOD is 


-Task to handle OOD functions 


task body THE.OOD is 

GOAL_FLAG_l : BOOLEAN := FALSE; -Flags for lower level objects 
RETURN_FLAG_1 : BOOLEANFALSE; 

OOD_X: FLOAT; 

OOD_Y: FLOAT; 

OOD.DEPTH: FLOAT; 

OOD HEADING ; FLOAT; 

OODSPEED; FLOAT; 

OOD.MODE: INTEGER; 

begin 

loop 

-Flags for lower level objects are checked for each command or predicate 

-query and then the result is sent back to the Strategic level 

select 

-Create tactical level objects 
accept CREATE; 

PUT_LINE(“Creating OOD”); 

THE_MISSION_MODEL.CREATE; 

THE_WORLD_MODEL.CREATE; 

THE_SENSORY_RECEI VER.CRE ATE; 
THE_OOD_ROUTER.CREATE; 

THE_NAVIGATOR.CREATE; 

THE.ENGINEERING.CREATE; 

THE.WEAPONS.CREATE; 

or 

accept READY_VEHICLE_FOR_LAUNCH(G_FLAG ; out INTEGER) do 
THE_WORLD_MODEL.INITIALIZE(GOAL_FLAG_l); 
if (GOAL_FLAG_l = TRUE) then 
THE_MISSION_MODEL.INITIALIZE(GOAL_FLAG 1); 
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if (G0AL_FLAG_1 = TRUE) then 
G.FLAG := 1; 

G0AL_FLAG_1 := FALSE; 
else 

G_FLAG := 0; 
end if; 
else 

G_FLAG := 0, 
end if; 

end READY.VEHICLE_FOR_LAUNCH; 
or 

accept SELECT_FIRST_WAYPOINT(G_FLAG : out INTEGER) do 
THE_NAVIGATOR.SELECT_FIRST_WAYPOINT(GOAL_FLAG_l); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0: 
end if; 

end SELECT.FIRST.WAYPOINT; 
or 

accept ALERT_USER(G_FLAG : out INTEGER) do 
PUT_LINE(“Failure detected during initialization.”); 

G.FLAG;» 1; 
end ALERT.USER; 
or 

accept IN_TRANSIT_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_MISSION_MODEL.IN_TRANSIT_P(GOAL_FLAG_l. RETURN.FL AG. 1); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG ;= 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN.FL AG. 1 = TRUE) then 
R.FLAG ;= 1; 

RETURN.FL AG. 1 := FALSE; 
else 

R.FLAG := 0; 
end if; 

end IN.TRANSIT.P; 
or 

accept TRANSIT_DONE_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_MISSION_MODEL.TRANSIT_DONE_P(GOAL_FLAG_l, RETURN.FLAG.l); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG ;= 0; 
end if; 

if (RETURN.FLAG.l = TRUE) then 
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R_FLAG:» 1; 

RETURN_FLAG_1:» FALSE; 
else 

R_FLAG := 0; 
end if; 

end TRANS IT_DONE_P; 

* 

accept IN_SEARCH_P(G_FLAG. R.FLAG : out INTEGER) do 
THE_mFsSION_MODEL.IN_SEARCH_P(GOAL_FL AG_1, RETURN.FL AG_ 1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG ;= 1; 

GOAL_FLAG_l :* FALSE; 
else 

G_FLAG := 0; 
end if; 

if (RETURN_FLAG_1 = TRUE) then 
R.FLAG := 1; 

RETURNFL AG_ 1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end IN_SEARCH_P; 

>r 

accept SEARCH_DONE_P(G_FLAG. R.FLAG : out INTEGER) do 
THE_MISSION_MODEL.SEARCH_DONE_P<GOAL_FLAG_ 1, RETURN_FLAG_1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG ;= 0; 
end if; 

if (RETURN_FLAG_1 = TRUE) then 
RJFLAG := 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end SEARCH .DONE _P; 
rr 

accept IN_TASK_P(G_FLAG, R.FLAG: out INTEGER) do 
THE_M3SSION_MODEL. IN_T AS K_P(GO AL_FL AG_ 1, RETURN_FLAG_1); 
if (GOAL_FLAG_l = TRUE) then 
G.FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN_FLAG_1 * TRUE) then 
R.FLAG := 1; 

RETURN.FL AG_ 1 ;= FALSE; 
else 
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R_FLAG := 0; 
end if; 

end IN_TASK_P; 
x 

accept TASK_DONE_P(G_FLAG, R_FLAG ; out INTEGER) do 
THE_MIS SION_MODEL.TASK_DONE_P(GOAL_FLAG_ 1, RETURN.FL AG. 1); 
if (GOAL.FLAG.l * TRUE) then 
G.FLAG := 1; 

GOAL_FLAG_l:» FALSE; 
else 

G.FLAG ;= 0; 
end if; 

if (RETURN_FL AG_ 1 = TRUE) then 
R_FLAG := 1; 

RETURN_FL AG_ 1 ;= FALSE; 
else 

R_FLAG ;= 0; 
end if; 

end TASK_DONE_P; 
yr 

accept IN_RETURN_P(G_FLAG, R_FLAG : out INTEGER) do 
THE_MISSION_MODEL.IN_RETURN_P(GO AL.FL AG. 1, RETURN.FL AG.l); 
if (GOAL_FLAG_l = TRUE) then 
G.FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN.FL AG. 1 = TRUE) then 
R.FLAG := 1; 

RETURN.FL AG. 1 := FALSE; 
else 

R.FLAG := 0; 
end if; 

end IN.RETURN.P: 
yf 

accept RETURN_DONE.P(G_FLAG, R.FLAG : out INTEGER) do 
THE_MISSION_MODEL.RETURN_DONE_P(GOAL_FLAG_l, RETURN.FL AG. 1); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0: 
end if; 

if (RETURN.FL AG. 1 = TRUE) then 
R.FLAG := 1; 

RETURN.FL AG. 1 := FALSE; 
else 

R.FLAG := 0; 
end if; 

end RETURN.DONE.P; 
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accept WArr_FOR_RECOVERY(G_FLAG : out INTEGER) do 
THE.NA V1G ATOR. WAIT.FOR.RECO VER Y (GO AL_FLAG_ 1 ); 
if (GOAL_FLAG_l * TRUE) then 

G.FLAG .* I; ~ 

GOAL.FLAG.l:» FALSE; 
else 

G.FLAG := 0; 
end if; 

end WAJT.FOR.RECOVERY; 

w 

accept SURFACE(G_FLAG ; out INTEGER) do 
THE_NAVIGATOR.SURFACE(GOAL_FLAG_l); 
if (GOAL.FLAG.l = TRUE) then 

G.FLAG := 1; 

GOAL.FLAG.l :» FALSE; 
else 

G.FLAG := 0; 
end if; 

end SURFACE; 

)T 

accept DO_SEARCH_PATTERN(G.FLAG : out INTEGER) do 
THE_NAVIGATOR.DO_SEARCH_PATTERN(GOAL_FLAG_1); 
if (GOAL.FLAG.l = TRUE) then 

G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0; 
end if; 

end DO.SEARCH.PATTERN; 

rr 

accept HOMING(G_FLAG ; out INTEGER) do 
THE.NA VIG ATOR.DO_HOMING(GOAL_FL AG. 1); 
if (GOAL.FLAG.l = TRUE) then 

G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0: 
end if; 

end HOMING; 

IT 

accept DROP_PACKAGE(G_FLAG : out INTEGER) do 
THE_WEAPONS.DROP_PACKAGE(GO AL.FL AG.l); 
if (GOAL.FLAG.l = TRUE) then 

G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0; 
end if; 

end DROP.PACKAGE; 
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accept GET_GPS_FIX(G_FLAG : out INTEGER) do 
THE_NAVIGATOR.GET_GPS_FIX(GOAL_FLAG_ 1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG ;= 0; 
end if; 

end GET_GPS_FIX; 
or 

accept GET_NEXT_WAYPOINT(G_FLAG : out INTEGER) do 
THE_NAVIGATdR.GET_NEXT_WAYPOINT(GOAL_FLAG_l); 
if (GOAL_FLAG_l = TRUE) then 
GJFLAG:* 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

end GET_NEXT_W A YPOINT; 
or 

accept SEND_SETPOINTS_AND_MODES(G_FLAG : out INTEGER) do 
select 

THE_NAVIGATOR.SEND_SETPOINTS_AND_MODES(GOAL_FLAG_l); 

or 

delay 1.0; 
end select; 

if (GOAL_FLAG_l = TRUE) then 
G_FLAG ;= 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

end SEND_SETPOINTS_AND_MODES; 
or 

accept REACH_WAYPOINT_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_N A VIG ATOR.RE ACH_W A Y POINT_P(GO AL_FL AG_ 1. RETURN_FL AG_ 1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG := 0; 
end if; 

if (RETURN_FLAG_1 = TRUE) then 
R_FLAG := 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end REACH_WAYPOINT_P; 
or 

accept GPS_NEEDED_P(G_FLAG, R_FLAG: out INTEGER) do 
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THE_NA VIG ATOR.GPS_NEEDED_P(GOAL_FL AG_ 1, RETURN_FL AG_ 1); 
if (G0AL_FLAG_1 * TRUE) then 

G.FLAG := 1; 

G0AL_FLAG_1 := FALSE; 
else 

G_FLAG := O, 
end if; 

if (RETURN_FLAG_1 = TRUE) then 

R_FLAG ;= 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

RJFLAG := 0; 
end if; 

end GPS_NEEDED_P; 

yr 

accept UNKNOWN_OBSTACLE_P(G_FLAG, R_FLAG : out INTEGER) do 
THE_NAVIGATOR.UNKNOWN_OBSTACLE_P(GOAL_FLAG_l. RETURN_FL AG_1); 
if (GOAL_FLAG_l = TRUE) then 

G_FLAG := 1; 

GOALJFLAG.l := FALSE; 
else 

G JFLAG := 0, 
end if; 

if (RETURNJFL AG_ 1 = TRUE) then 

RJFLAG := 1; 

RETURN.FLAGJ := FALSE; 
else 

R_FLAG := 0; 
end if; 

end UNKNOWN_OBSTACLE_P; 

>r 

accept LOG_NEW_OBSTACLE(G_FLAG : out INTEGER) do 
THE_NA VIG ATOR.LOG_NEW_OBSTACLE(GOAL_FL AG_ 1); 
if (GOAL JFLAG. 1 = TRUE) then 

G_FLAG ;= 1; 

GOAL_FLAG_l ;= FALSE; 
else 

G.FLAG := 0; 
end if; 

end LOG_NEW_OBSTACLE; 

w 

accept LOITER(G_FLAG : out INTEGER) do 
G_FLAG := 1; 

end LOITER; 

n 

accept START_LOCAL_REPLANNER(G_FLAG : out INTEGER) do 
THE_NAVIGATOR.START_LOCAL_REPLANNER(GOAL_FLAG_l); 
if (GOAL_FLAG_l = TRUE) then 

G_FLAG := 1; 

GOAL_FLAG_l:« FALSE; 
else 


56 








G_FLAG := 0, 
end if; 

end START_LOCAL_REPLANNER; 

accept START_GLOBAL_REPLANNER(G_FLAG :out INTEGER) do 
THE_NAVlGATOR.START_GLOBAL_REPLANNER(GOAL_FLAG_l); 
if (GOAL_FLAG_l = TRUE)'then 

G_FL AG ;= 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG := 0, 
end if; 

end START_GLOBAL_REPLANNER; 

yf 

accept POWER_GONE_P(G_FLAG, R_FLAG ; out INTEGER) do 
THE_ENGINEERING.POWER_GONE_P(GO AL_FL AG_ 1, RETURN_FL AG_ 1); 
if (G0AL_FLAG_1 = TRUE) then 

G_FLAG := 1; 

G0ALJFLAG_1 := FALSE; 
else 

G_FLAG := 0; 
end if; 

if (RETURN_FLAG_1 = TRUE) then 

R_FLAG := 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

R_FLAG := 0: 
end if; 

end POWER_GONE_P: 


accept COMPUTER_SYSTEM_PROB_P(G_FLAG, R_FLAG : out INTEGER) do 
THE_ENGINEERING.COMPUTER_SYSTEM_PROB_P(GOAL_FLAG_l,RETURN_FLAG_l); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG:= 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN_FLAG_1 = TRUE) then 
R.FLAG := 1; 

RETURNFLAG.l := FALSE; 
else 

R.FLAG := 0; 
end if; 

end COMPUTER_S Y STEM_PROB_P; 

it 

accept PROPULSION_SYSTEM_PROB_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_ENGINEERINGi > ROPULSION_SYSTEM_PROB_P(GOAL_FLAG_l,RETURN_FLAG_l); 
if (GOAL_FLAG_l ■ TRUE) then 
G_FLAG := 1; 
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GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN_FL AG_ 1 = TRUE) then 
R_FLAG ;= 1; 

RETURN_FLAG_1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end PROPULSION_S YSTEM_PROB_P; 

H 

accept STEERING_SYSTEM_PROB_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_ENGINEERING.STEERING_SYSTEM_PROB_P(GOAL_FLAG_l, RETURN_FLAG_1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG ;= 0; 
end if; 

if (RETURNJFLAG_1 = TRUE) then 
R_FLAG := 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end STEERING_SYSTEM_PROB_P; 

>r 

accept DIVING_SYSTEM_PROB_P(G_FLAG, R_FLAG : out INTEGER) do 
THE_ENGINEERJNG .DI VING_S Y STEM_PROB_P(GO AL_FL AG_ 1. RETURN_FL AG_ 1); 
if (GOAL_FLAG_l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURNFLAG.l = TRUE) then 
R.FLAG ;= 1; 

RETURNJFLAG_1 := FALSE; 
else 

R_FLAG := 0; 
end if; 

end DIVING_SYSTEM_PROB_P; 
n 

accept BUOYANCY_SYSTEM_PROB_P(G_FLAG, R.FLAG ; out INTEGER) do 
THE_ENGINEERING.BUOYANCY_SYSTEM _PROB_P(GOAL_FLAG _1, RETURN_FL AG_ 1); 
if (GOAL JFLAG_1 = TRUE) then 
GJFLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G.FLAG := 0; 
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end if; 

if (RETURN.FL AG„ 1 = TRUE) then 
R_FLAG := 1; 

RETURN_FL AG_ 1 := FALSE; 
else 

R_FLAG ;= 0; 
end if; 

end BUOYANCY_SYSTEM_PROB_P; 
rr 

accept THRUSTER_SYSTEM_PROB_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_ENGINEERING.THRUSTER_SYSTEM_PROB_P(GOAL_FLAG_l, RETURN_FLAG_1); 
if (GOAL.FLAG.l = TRUE) then 
G_FLAG := 1; 

GOAL_FLAG_l := FALSE; 
else 

G_FLAG := O, 
end if; 

if (RETURN_FL AG_ 1 = TRUE) then 
R_FLAG := 1; 

RETURN.FL AG. 1 := FALSE; 

else 

R.FLAG := 0; 
end if; 

end THRUSTER_SYSTEM_PROB_P; 

>r 

accept LEAK_TEST_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_ENGINEERING.LEAK_TEST_P(GOAL_FLAG_l. RETURN.FL AG. 1); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN.FLAG. 1 = TRUE) then 
R.FLAG := 1; 

RETURN.FL AG. 1 := FALSE; 
else 

R.FLAG := 0; 
end if; 

end LEAK.TEST.P; 

IT 

accept PAYLOAD_PROB_P(G_FLAG, R.FLAG : out INTEGER) do 
THE_ENGlNEERING.PAYLOAD_PROB_P(GOAL_FLAG_l, RETURN.FL AG. 1); 
if (GOAL.FLAG.l = TRUE) then 
G.FLAG := 1; 

GOAL.FLAG.l := FALSE; 
else 

G.FLAG := 0; 
end if; 

if (RETURN.FL AG. 1 = TRUE) then 
R.FLAG := 1; 
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RETURN_FL AG_ 1 := FALSE; 
else 

R.FLAG := 0; 
end if; 

end PAYLOAD _PROB_P; 
end select; 
end loop; 
end THE_OOD; 

end OOD; 
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--Title : ood_r_s.a (CLIPS-Ada Simulator Version) 
-Author : FP. Thom ton Jr. 

-•Revised : 26 Aug 93 

--Compiler : VADS 

--System : Unix 

--Description : Specification for OOD Router task 


package OOD_ROUTER is 

task THE_OOD_ROUTER is 
entry CREATE; 

entry TAKE_NAV_COMMANDSfWAYPOINT_X : in FLOAT; 

WAYPOINT_Y : in FLOAT; 
NAV_HEADING ; in FLOAT; 
NAV.SPEED: in FLOAT; 

NAV_DEPTH: in FLOAT; 

NAV_MODE: in INTEGER); 

entry TAKE_GU1DANCE_C0MMANDS(NAV_HEADING : in FLOAT: 

NAV_MODE : in INTEGER); 

end THE_OOD_ROUTER; 
end OOD_ROUTER; 
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-Title : ood_r_b.a (CLIPS*Ada Simulator Version) 

-Author : F.P. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Body for OOD Router task 


with TEXT JO, MISSION.MODEL, COMMAND.SENDER; 
use TEXTJO; 

package body OOD_ROUTER is 


-Task to handle routing of requests to OOD, required to allow time-consuming 
-tasks to continue (search, homing, replanning) 


task body THE_OOD_ROUTER is 
OOD_X : FLOAT; 

OOD_Y; FLOAT; 

OOD_HEADING ; FLOAT; 

OOD.SPEED ; FLOAT; 

OOD_DEPTH; FLOAT; 

OOD.MODE : INTEGER; 
begin 

accept CREATE; 

PUT_LINE( , 'Creating OOD ROUTER"); 
loop 
select 

-Get Navigator commands to send to Command Sender 
accent TAKE_NAV_COMMANDS (W AYPOINT_X : in FLOAT; 

WAYPOINT_Y: in FLOAT; 

NAV_HEADING: in FLOAT; 

NAV_SPEED; in FLOAT; 

NAV.DEPTH : in ROAT; 

NAV_MODE : in INTEGER) do 

OOD_X :■ WAYPOINT.X: 

OOD_Y := WAYPOINT_Y; 

OOD.HEADING := NAV_HEADING; 

OOD_SPEED := NAV_SPEED; 

OOD_DEPTH := NAV_DEPTH; 

OOD_MODE := NAV_MODE; 
end TAKE_NAV_COMMANDS; 

COMMAND_SENDER_SEND(OOD_X. OOD_Y,OOD_HEADING, OOD.SPEED, 

OOD.DEPTH, OOD.MODE); 
or 

accept TAKE_GUIDANCE_COMMANDS(NAV_HEADING : in ROAT; 

NAV.MODE: in INTEGER) do 
OOD.HEADING := NAV_HEADING; 

OOD.MODE := NAV_MODE; 
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end TAKE J3UIDANCECOMMANDS; 

COMMAND_SENDER.SEND(OOD_X. OOD_Y. OOD_HEADING. OOD_SPEED. 

OOD.DEPTH, OOD_MODE); 

end select; 
end loop; 

end THE.OOD.ROUTER; 
end OOD_ROUTER; 
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-Title : nav_b.a (CLIPS-Ada Simulator Version) 

-Author : FJ*. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for Navigator task 


package NAVIGATOR is 

task THE_NAV1GAT0R is 
entry CREATE; 

entry SELECT_FIRST_WAYPOINT(G_FLAG_l : out BOOLEAN); 
entry WAIT_FOR_RECOVERY(G_FLAG_l: out BOOLEAN); 
entry SURFACE(G_FL AG_ 1: out BOOLEAN); 
entry DO_SEARCH_PATTERN(G_FLAG_ 1: out BOOLEAN); 
entry DO_HOMING(G_FLAG_l : out BOOLEAN); 
entry GET_GPS_F1X(G_FLAG_1 : out BOOLEAN); 
entry GPS_NEEDED_P(G_FLAG_1, R_FLAG_1 : out BOOLEAN); 
entry GET_NEXT_WAYPOINT(G_FLAG_ 1 : out BOOLEAN): 
entry REACH_WAYPOINT_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN): 
entry SEND_SETPOINTS_AND_MODES<G_FLAG J ; out BOOLEAN): 
entry UNKNOWN_OBSTACLE_P(G_FLAG_ 1. R_FLAG_1 : out BOOLEAN); 
entry LOG_NEW_OBSTACLE(G_FLAG_l : out BOOLEAN): 
entry START_LOCAL_REPLANNER(G_FLAG_l ; out BOOLEAN): 
entry START_GLOB AL_REPL ANNER(G_FL AG_ 1 : out BOOLEAN): 
end THE_NAVIGATOR; 

end NAVIGATOR; 



--Title : nav_b.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Body for Navigator task 


with TEXT JO. MATH. MISSION.MODEL. SENSOR Y_RECEIVER. OOD_R OUTER. 

NAVIGATOR.ROUTER. GUIDANCE. GPS.CONTROL, SONAR.CONTROL. 
MISSION.REPLANNER; 

use TEXT JO. MATH, MISSION.MODEL. SENSOR Y.RECEIVER, OOD.ROUTER. 

NAV1GATOR.ROUTER. GUIDANCE. GPS.CONTROL. SONAR.CONTROL. 
MISSION.REPLANNER; 

package body NAVIGATOR is 


-Task to handle navigation functions 


task body THE.NAVIGATOR is 

GOAL.FLAG.2 : BOOLEAN := FALSE; -Flags for lower level objects 
RETURN.FLAG.2 : BOOLEAN := FALSE; 

STARTED : BOOLEAN := FALSE; -Flag to start comm protocol 
REPEATED : BOOLEAN := FALSE; -Flag to continue comm protocol 
NAV.X: FLOAT; 

NAV.Y ; FLOAT; 

NAV.DEPTH: FLOAT; 

NAV.HEADING : FLOAT; 

NAV.SPEED: FLOAT: 

NAV.MODE : INTEGER; 

NAV.BEARING : FLOAT: 

NAV.RANGE : FLOAT; 

WAYPOINT.X: FLOAT: 

WAYPOINT.Y : FLOAT; 

WAYPOINTJ)EPTH: FLOAT: 

EPSILON : constant FLOAT := 20.0; -Tolerance for achieving waypoint 
SURFACE.LIMIT: constant FLOAT := 5.0; -Tolerance for Surface condition 

begin 

-Create Navigator’s subobjects 
accept CREATE; 

PUT.LINErCreating NAVIGATOR’): 
THE.NAVIGATOR.ROUTER.CREATE: 

THE.GUIDANCE.CREATE; 

THE.GPS_CONTROL.CRE ATE: 

THE.MISSION.REPLANNER.CREATE; 

THE.SON AR.CONTROL.CRE ATE; 

-Receive initial state and Fust waypoint 
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accept SELECT_FIRST_WAYPOINT(G_FLAG_ 1: out BOOLEAN) do 
THE.MIS S ION.MODEL.GI VE.FIRST. W A YPOINTfNA V_X, NAV.Y. NAV_DEPTH. 
NAV.MODE, 

NAV_HEADING.NAV_SPEED. WAYPOINT_X. 

W A YPOINT. Y, WAYPOINT.DEPTH); 

G.FLAG.l :* TRUE; 
end SELECT.FIR ST.WAYPOINT; 
loop 
select 

accept WATT_FOR_RECOVERY(G_FLAG_ 1 : out BOOLEAN) do 
G.FLAG.l := TRUE; 
end WAJT.FOR.RE CO VER Y; 

-Loop under Tactical level control until signaled for mission 

--download 

loop 

-Delay to comply with simulator Tactical-Execution comm protocol 
-For every set of data received a set of commands must be sent 
delay 0.2; 

THE.SENSORY.RECEIVER.RECEIVEfNAV.X. NAV.Y. NAV.DEPTH, NAV.HEADING. 

NAV.BEARING. NAV.RANGE); 

WAYPOINT.DEPTH ;= 0.0; 

NAV.SPEED ;= 0.0; 

THE.OOD.ROUTER.T AKE.N A V.COMMANDS (W AYPOINT.X. W A YPOINT. Y. 
NAV.HEADING. 

NAV.SPEED. WAYPOINT.DEPTH. NAV.MODE); 

end loop; 
or 

accept SURFACE(G_FLAG_1 : out BOOLEAN) do 
loop 

-Simulator protocol delay 
delay 0.2; 

THE_SENSORY_RECEIVER.RECEIVE(NAV_X. NAV.Y. NAV.DEPTH. NAV.HEADING. 

NAV.BEARING. NAV.RANGE); 
exit when NAV.DEPTH < SURFACE.LIMIT; 

WAYPOINT.DEPTH := 0.0; 

THE.OOD.ROUTER.T AKE.N A V.COMMANDS (W AYPOINT.X. W A YPOINT. Y, 
NAV.HEADING. NAV.SPEED. 

WAYPOINT.DEPTH. NAV.MODE); 

end loop; 

G.FLAG.l :=TRUE; 
end SURFACE; 
or 

accept DO_SEARCH_PATTERN(G_FLAG_l : out BOOLEAN) do 
THE_SONAR_CONTROL.DO_SEARCH_PATTERN(GOAL_FLAG_2, NAV.HEADING); 
if (GOAL.FLAG.2 = TRUE) then 
G.FLAG.l := TRUE; 

GOAL.FLAG.2 := FALSE: 
else 

G.FLAG.l := FALSE; 
end if: 

end DO.SE ARCH.PATTERN; 







accept DO_HOMING(G_FLAG_l : out BOOLEAN) do 
THE_GlilDANCE.DO_HOMING(GOAL_FLAG_2); 
if (GOAL_FLAG_2 « TRUE) then 
G_FLAG_1 :« TRUE; 

GOAL_FLAG_2 := FALSE; 
else 

G_FLAG_1 :« FALSE; 
end if; 

end DO.HOMING; 

)r 

accept GET_GPS_FIX(G_FLAG_1 : out BOOLEAN) do 
THE_GPS_CONTROL.GET_GPS_FIX(GOAL_FLAG_2); 
if (GOAL_FLAG_2 * TRUE) then 
G.FLAG.l ;= TRUE; 

GOAL_FLAG_2:« FALSE: 
else 

G_FLAG_1 := FALSE: 
end if: 

end GET_GPS_FIX; 

>r 

accept GPS.NEEDED_P(G_FLAG_1. R.FLAG.l : out BOOLEAN) do 
G_FLAG_1 :« FALSE; 

R_FLAG_1 := TRUE: 
end GPS_NEEDED_P: 
w 

accept GET_NEXT_WAYPOINT(G.FL AG. 1 : out BOOLEAN) do 
THE MlSSION_MODEL.GIVE_NEXT_WAYPOINT(WAYPOINT_X.WAYPOINT_Y. 

waypoint.depth. NAV_SPEED. 
NAV.MODE): 

G.FLAG.l := TRUE; 
end GET_NEXT_W A YPOINT; 

>r 

accept REACH_WAYPOINT_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
if SQRT((WAYPOINT_X - NAV_X)**2 + (WAYPOINT.Y - NAV_Y)**2) 

< EPSILON then --Reached waypoint 
G_FLAG_1 := TRUE; 

PUT_LINEr*****At waypoint, coming to new heading*****"): 
else 

G.FLAG.l := FALSE; 
end if; 

R.FLAG.l := TRUE; 
end REACH.WAYPOINT.P; 

-Do guidance in the background 
if not REPEATED then -Update navigation 
if STARTED then 

-Get current status values from Sensory Receiver 

THE.SENSORY.RECEIVER.RECEIVE(NAV.X, NAV.Y, NAV.DEPTH, NAV.HEADING, 

NAV.BEARING, NAV.RANGi); 

end if; 

-Send for new commands from Guidance 
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THE_GUIDANCE.GET_GUIDANCE_COMMANDS(NAV_X, NAV_Y, NAV.DEPTH. 
NAV.HEADINGNAV.SPEED, WAYPOINT.X. 

WAYPOINT.Y. WAYPOINT DEPTH); 

STARTED:» TRUE; 

REPEATED :• TRUE; 
end if; 
or 

accept SEND_SETPOINTS_AND_MODES(G_FLAG_l: out BOOLEAN) do 
THE_OOD_ROUTER.TAKE_NAV_COMMANDS(W A YPOINT.X, WAYPOINT.Y, 

NAV.HEADING, NAV.SPEED, 

NAV.DEPTH, NAV MODE); 

G.FLAG.l := TRUE; 

REPEATED := FALSE; 
end SEND.SETPOINTS.AND.MODES: 
or 

accept UNKNOWN_OBSTACLE_P(G_FLAG_l. R.FLAG.l: out BOOLEAN) do 
THE_SONAR_CONTROL.UNKNOWN_OBiTACLE_P(GOAL_FLAG_2,RETURN_FLAG_2); 
if (GOAL.FLAG.2 = TRUE) then 
G.FLAG.l := TRUE; 

GOAL.FLAG.2 ;= FALSE, 
else 

G.FLAG.l := FALSE; 
end if; 

if (RETURN.FLAG.2 = TRUE) then 
R.FLAG.l := TRUE; 

RETURN.FLAG.2 := FALSE; 
else 

R.FLAG.l := FALSE; 
end if: 

end UNKNOWN.OBSTACLE.P; 
or 

accept LOG_NEW_OBSTACLE(G_FLAG_l : out BOOLEAN) do 
THE_SONAR_CONTROL.LOG_NEW_OBSTACLE(GOAL_FLAG_2); 
if (GOAL.FLAG.2 = TRUE) then 
G.FLAG.l := TRUE; 

GOAL.FLAG.2 := FALSE; 
else 

G.FLAG.l := FALSE; 
end if; 

end LOG.NEW.OBSTACLE; 
or 

accept START_LOCAL_REPLANNER(G_FLAG_l : out BOOLEAN) do 
THE.MISSION_REPLANNER.ST ART.LOC AL.REPLANNER; 

THE_GUIDANCEJLOITER(NAV_X, NAV.Y, NAV.DEPTH, NAV HEADING, NAV SPEED. 

NAV.MODE); 

G.FLAG.l := TRUE; 
end START.LOC AL.REPLANNER: 
or 

accept START_GL0BAL_REPLANNER(G_FLAG_1: out BOOLEAN) do 
THE_MISSION_REPLANNER.START.GLOB AL.REPLANNER; 

THE.GUID ANCE .LOITER(N A V.X. NAV.Y, NAV.DEPTH. NAV.HEADING. NAV.SPEED. 
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NAV_MODE); 
G.FLAG.l :» TRUE: 
end START_GLOB AL_REPLANNER: 
end select; 
end loop; 

end THE.NAVIGATOR; 


end NAVIGATOR; 






-Title : nav_r_s.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Specification for Navigator Router task 


package NAV1GATOR.ROUTER is 
task THE_NAVlGATOR_ROUTER is 
entry CREATE; 

entry TAKE_GUIDANCE_HEADING(GUIDANCE_HEADING : in FLOAT; 

GUIDANCE.MODE: in INTEGER); 
entry TAKE_LOITER_COMMANDS(GUIDANCE_X : in FLOAT; 
GUIDANCE_Y: in FLOAT: 

GUIDANCE.HEADING : in FLOAT; 

GUIDANCE.SPEED: in FLOAT; 

GUIDANCE.DEPTH : in FLOAT; 

GUIDANCE.MODE; in INTEGER; 
LOITER_GUIDANCE_DONE: out BOOLEAN): 
entry REPLAN.DONE; 
end THE_NAVlGATOR_ROUTER; 

end NAVIGATOR_ROUTER: 
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-Title : nav_r_b.a (CLIPS-Ada Simulator Version) 
-Author : FP. Thornton Jr. 

-Revised : 17 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Body for Navigator Router task 


with TEXTJO, OOD.ROUTER; 
use TEXTJO, OOD.ROUTER; 

package body NAVIGATOR JIOUTER is 


-Task to handle routing of requests through Navigator 


task body THE_NAVlGATOR_ROUTER is 


NAV_X : FLOAT; 

NAV_Y : FLOAT; 

NAV_DEPTH: FLOAT; 

NAV_SPEED: FLOAT; 

NAV_HEADING : FLOAT; 

NAV_MODE : INTEGER; 

NAV_REPLAN_DONE ; BOOLEAN := FALSE; -Flag to signal replan done 


begin 

accept CREATE; 

PUT_LINE(”Creating NAVIGATOR ROUTER”): 
loop 
select 

accept TAKE_GUIDANCE_HEADING(GUIDANCE_HEADING : in FLOAT; 

GUIDANCE.MODE : in INTEGER) do 
NAV_HEADENG := GUIDANCE_HEADING; 

NAV MODE :m GUIDANCE_MODE; 
end TAKE_GUIDANCE_HEADING: 

-In Search mode so take search commands immediately 

THE_OOD_ROUTER.TAKE_GU1DANCE_COMMANDS(NAV_HEADING. NAV_MODE); 
or 

accept TAKE_LOITER_COMMANDS(GUIDANCE_X : in FLOAT; 

GUIDANCE_Y; in FLOAT; 

GUIDANCE JIEADING ; in FLOAT; 

GUIDANCE.SPEED: in FLOAT; 

GUIDANCEJDEPTH : in FLOAT; 

GUIDANCE.MODE: in INTEGER; 

LOITER_GUIDANCE_DONE : out BOOLEAN) do 
NAV_X := GUIDANCE J<; 

NAV_Y := GUIDANCE.Y; 

NAV.HEADING := GUIDANCE.HEADING; 

NAV.SPEED := GUIDANCE.SPEED: 


71 






NAV.DEPTH :* GUIDANCE.DEPTH: 

NAV_MODE := GUIDANCE_MODE; 

LOrTER_GUIDANCE_DONE :* NAV_REPLAN_DONE; 
end TAKE J.OITER_COMMANDS; 

THE_OOD_ROUTER.TAKE_NAV_COMMANDS(NAV_X, NAV_Y. NAV_HEADING. 

NAV_SPEED. NAV_DEPTH. NAV_MODE); 
or 

accept REPLAN_DONE; 

NAV_REPLAN_DONE := TRUE; 
end select; 
end loop; 

end THE_NAVIGATOR_ROUTER; 
end NAVIGATOR_ROUTER; 
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-Tide 

--Author 

-Revised 

-Compiler 

-System 

-Description 


: engin_s.a 
: FP. Thornton Jr. 

: 26 Aug 93 
: VADS 
: Unix 

: Specification for Engineering task 


package ENGINEERING is 

task THEJENGINEERING is 
entry CREATE; 

entry POWER_GONE_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN); 
entry COMPUTER_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN); 
entry PROPULSION_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN); 
entry STEERENG_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_ 1 : out BOOLEAN); 
entry DIVD9G_SYSTEM_PROB_P(G_FLAG_l, R_FLAG_1 : out BOOLEAN); 
entry BUOYANCY_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 ; out BOOLEAN); 
entry THRUSTER_SYSTEM_PROB_P(G_FLAG_1. R_FLAG_1 ; out BOOLEAN): 
entry LEAK_TEST_P(G_FLAG_ 1. R_FLAG_1 : out BOOLEAN); 
entry PAYLOAD_PROB_P(G_FLAG_l. R_FLAG_1 ; out BOOLEAN); 
end THE_ENGINEERING; 

enu ENGINEERING; 


73 




--Title ; engin.b.a 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

--System : Unix 

--Description : Body for Engineering task 


with TEXT JO. MATH. CALENDAR; 
use TEXT JO, MATH. CALENDAR; 

package body ENGINEERING is 


-Task to handle engineering functions such as monitoring onboard systems 


task body THE JENGINEERING is 

THRUSTERJ.EVEL : FLOAT := 100.0; 

THRUSTER_MIN : FLOAT ;= 80.0; 

THRUSTERJ-OSS ; FLOAT := 1.0; 

begin 

accept CREATE; 

PUT_LINE(“Creating ENGINEERING”); 
loop 
select 

accept POWER_GONE„P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
G_FLAG_1 ;= FALSE; 

R_FLAG_1 := TRUE; 
end POWER J30NE_P: 
or 

accept COMPUTER_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
end COMPUTER_SYSTEM_PROB_P; 
or 

accept PROPULSION_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 ; out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
end PROPULSION_SYSTEM_PKOB_P; 
or 

accept STEERING_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_I : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
end STEERING_SYSTEM_PROB_P; 
or 

accept DIVING_SYSTEM_PROB_P(G_FLAG_ 1. R_FLAG_ 1 : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
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end DIVING_S YSTEM_PROB_P; 
or 

accept BUOYANCY_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R FLAG.l := TRUE; 
end BUOYANCY_SYSTEM_PROB_P; 
or 

accept THRUSTER_SYSTEM_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
if THRUSTER J.EVEL >THRUSTER_MIN then 
THRUSTER.LEVEL := THRUSTER_LEVEL - THRUSTER.LOSS; 

G_FLAG_1 := FALSE; 
else 

G_FLAG_1 := TRUE; 
end if; 

R_FLAG_1 ;= TRUE; 
end THRUSTER_SYSTEM_PROB_P; 
or 

accept LEAK_TEST_P(G_FLAG_ 1, R_FLAG_1 : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
end LEAK_TEST_P; 
or 

accept PAYLOAD_PROB_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN) do 
G_FLAG_1 := FALSE; 

R_FLAG_1 := TRUE; 
end PAYLOAD_PROB_P; 
end select; 
end loop; 

end THE_ENGINEERING: 


end ENGINEERING; 





--Title : weapon_s.a (CLIPS-Ada Simulator Version) 
-Author : F.P. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 
-System : Unix 

--Description : Specification for Weapons task 


package WEAPONS is 

task THE_WEAPONS is 
entry CREATE; 

entry DROP_PACKAGE(G_FLAG_l : out BOOLEAN); 
end THE_ WEAPONS; 

end WEAPONS; 
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-Tide 

-Author 

-Revised 

-Compiler 

-System 

-Description 


: weapon_ba (CLIPS-Ada Simulator Version) 
: FP. Thornton Jr. 

: 26 Aug 93 
: VADS 
: Unix 

: Body for Weapons task 


with TEXTJO; 
use TEXTJO; 

package body WEAPONS is 


-Task to handle functions of weapons officer 


task body THE_WEAPONS is 
begin 

accept CREATE; 

PUT_LINE("Creating WEAPONS"): 
loop 

accept DROP_PACKaGE(G_FLAG_ 1 : out BOOLEAN) do 
G_FLAG_1 := TRUE; 
end DROP.PACKAGE; 
end loop; 

end THE_ WEAPONS; 
end WEAPONS; 


t 
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-Title : sender_s.a (CLIPS-Ada Simulatoi Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for Command Sender 


package COMMAND_SENDER is 

procedure SEND(NEW_X : in FLOAT: 
NEW.Y: in FLOAT; 
NEW.HEADING : in FLOAT; 
NEW_SPEED: in FLOAT; 
NEW.DEPTH: in FLOAT; 
NEW_MODE: in INTEGER); 

end COMMAND_SENDER; 
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-Title : sender_b.a (CLIPS-Ada Simulator Version) 
-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

—System : Unix 

-Description : Body for Command Sender 


with TEXT_IO. MATH. TRIG.MATH. NETWORK.SW; 
use TEXT JO. MATH. TRIG.MATH, NETWORK.SW; 

package body COMMAND.SENDER is 

package FLOAT.INOUT is new FLOAT_IO(FLOAT); 
package INTEGER JNOUT is new INTEGER JO(INTEGER); 
use FLOAT.INOUT. INTEGER JNOUT: 


-Procedure to send tactical level information to the execution level 


procedure SEND(NEW_X : in FLOAT; 
NEW.Y : in FLOAT; 

NEWHE ADING : in FLOAT; 
NEW.SPEED; in FLOAT; 
NEW DEPTH: in FLOAT; 
NEW.MODE ; in INTEGER) is 


begin 

-Write updated command values to execution level 
PUT_FLOAT(RAD_TO_DEG(NEW_HE ADING)). 

PUTf'Commanded Heading is: “); 

PUT(RAD_TO_DEG(NEW_HEADING). FORE=>3.AFT=>2. EXP=>0); 
NEW.LINE; 

PUT_FLOAT(NEW_DEPTH); 

PUTC'Commanded Depth is: “): 

PUT(NEW_DEPTH. FORE=>3.AFT=>2. EXP=>0): 

NEW.LINE: 

PUT_FLOAT(NEW_SPEED): 

PUT(*‘Commanded Speed is:"); 

PUT(NEW.SPEED, FOR£=>3. AFT=>2. EXP=>0): 

NEW.LINE: 

PUT_FLOAT(NEW_X); 

PUTCCommanded X is: “): 

PUT(NEW_X. FORE=>3, AFT=>2. EXP=>0); 

NEW.LINE; 

PUT_FLOAT(NEW_Y); 
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PUT("Commanded Y is: '*); 

PUT(NEW_Y, FORE=>3, AFT=>2, EXP=>0): 

NEW.LINE; 

PUT_MODE(NEW_MODE); 

PUTCCommanded Mode is: 

case NEW MODE is 
when 1 «> 

PUTCTransit"): 
when 2 *> 

PUT(“Search”): 
when 3 => 

PUTCTask”): 
when 4 => 

PUTCRetum”); 
when 5 => 

PUT(“Recover”): 
when others => 

PUT(“Invalid Mode”): 

end case; 

NEW_LINE(2); 
end SEND; 

end COMMAND_SENDER: 
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-Title : guid_s.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for Guidance task 


package GUIDANCE is 

task THE_GUIDANCE is 
entry CREATE; 

entry GET_GUIDANCE_COMMANDS{NAV_X ; in out FLOAT: 
NAV_Y : in out FLOAT; 

NAV_DEPTH : in out FLOAT; 
NAV_HEADING : in out FLOAT; 
NAV_SPEED : in out FLOAT: 

WAYPOINTX : in out FLOAT; 
WAYPOINT_Y : in out FLOAT; 
WAYPOINT_DEPTH : in out FLOAT): 
entry LOITER(NAV_X : in FLOAT: 

NAV_Y : in FLOAT; 

NAV_DEPTH : in FLOAT: 

NAV_HEADING : in FLOAT; 

NAV_SPEED: in FLOAT; 

NAV_MODE : in INTEGER); 
entry DO_HOMING(G_FLAG_2 : out BOOLEAN): 
end THE_GUIDANCE: 

end GUIDANCE: 
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-Title : guid.b.a (CLIPS-Ada Simulator Version) 

--Author : F.P. Thornton Jr. 

-Revised : 26 Aug 93 

--Compiler : VADS 

--System : Unix 

-Description : Body for Guidance task 


with TEXT JO, SENSORYJtECEIVER. GUIDANCE.ROUTER. NAVIGATOR JtOUTER, 
LOS.CALCULATOR, HOMING.CALCULATOR; 
use TEXT JO, SENSOR Y.RECEIVER, GUIDANCE.ROUTER. NAVIGATOR_ROUTER; 

package body GUIDANCE is 


-Task to handle guidance functions such as Homing and LOS calculations 


task body THE.GUIDANCE is 


GOAL_FLAG_3 : BOOLEAN := FALSE: --Flag for lower level objects 

GUIDANCE.X : FLOAT; 

GUIDANCE.Y : FLOAT: 

GUIDANCE.DEPTH : FLOAT; 

GUIDANCE.WAYPOINT.X : FLOAT; 

GUIDANCEJVAYPOINT.Y : FLOAT: 

GUIDANCE. WAYPOINT.DEPTH: FLOAT: 

GUIDANCEJIEADING : FLOAT; 

GUIDANCEJ5PEED: FLOAT; 

GUIDANCE.MODE : INTEGER: 

GUIDANCE.BEARING : FLOAT: 

GU1D ANCE.P ANGE : FLOAT; 

LOITER.GUIDANCE.DONE : BOOLEAN := FALSE: -Flag to signal replanning done 


begin 

accept CREATE; 

PUT.LINEC'Creating GUIDANCE”): 

THE.GUIDANCE.ROUTER.CREATE; 

loop 

select 

accept GET_GUIDANCE_COMMANDS(N AV_X : in out FLOAT: 

NAV_Y : in out FLOAT; 

NAV.DEPTH : in out FLOAT; 

NAV.HEADING ; in out FLOAT; 

NAV.SPEED: in out FLOAT; 

WAYPOINT.X : in out FLOAT; 

WAYPOINT.Y : in out FLOAT; 

WAYPOENT.DEPTH: in out FLOAT) do 
LOS_CALCULATOR.DO_LOS_GUIDANCE(NAV_X, NAV.Y. NAV.DEPTH, 
WAYPOINT.X. WAYPOINT. Y, 

WAYPOINT.DEPTH. NAV.SPEED. 
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NAV.HEADING); 
end GET.GUIDANCE.COMMANDS; 

or 

accept DO_HOMING(G_FLAG_2 : out BOOLEAN) do 
HOMING_CALCULATOR.DO_HOMING_GUIDANCE(GOAL_FLAG_3); 
if (GOAL_FLAG_3 * TRUE) then 
G_FLAG_2:» TRUE; 

GOAL_FLAG_3 := FALSE; 
else 

G_FLAG_2;« FALSE; 
end if; 

end DO_HOMING; 
or 

accept LOITER(NAV_X : in FLOAT; 

NAV_Y ; in FLOAT; 

NAV_DEPTH: in FLOAT; 

NAV.HEADING: in FLOAT; 

NAV.SPEED: in FLOAT; 

NAV_MODE : in INTEGER) do 
GUIDANCE_WAYPOINT_X := NAV_X; 

GUIDANCE_WAYPOINT_Y := NAV_Y; 

GUTDANCE_WAYPOINT_DEPTH := NAV_DEPTH; 

GU1DANCE_HEADING ;= NAV.HEADING; 

GUIDANCE.SPEED ;= NAV_SPEED; 

GUIDANCE_MODE := NAV_MODE: 
loop 

--Simulator protocol delay 
delay 0.5; 

THE_SENSORY_RECEIVER.RECEIVE(GUIDANCE_X > GUIDANCE.Y. GUIDANCE.DEPTH. 
GUIDANCE.HEADENG. GU1DANCE_BEARING. 

GUIDANCE_RANGE); 

LOS_CALCULATOR.DO_LOS_GUlDANCE(GUIDANCE_X, GUIDANCE_Y. 

GUID ANCE_DEPTH. 

GU1DANCE_WAYP0INT_X, 

GUIDANCE_WAYPOINT_Y. 

GUID ANCE_W A YPOINT_DEPTH. 

GUIDANCE_SPEED, GUIDANCE.HEADING); 
THE_NAVlGATOR_ROUTER.TAKE_LOITER_COMMANDS(GUIDANCE_WAYPOINT_X. 
GUIDANCE_WAYPOINT_Y. 

GUIDANCE.HEADING, 

GUIDANCE.SPEED, 

GUIDANCE.WAYPOINT.DEPTH, 

GUIDANCE.MODE, 

LOITER.GUIDANCE.DONE); 
exit when LOITER.GUIDANCE.DONE: 
end loop; 
end LOITER; 
end select; 
end loop; 

end THE.GUIDANCE; 
end GUIDANCE; 
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-Title : guid_r_s.a (CLIPS-Ada Simulator Version) 

--Author : FJ\ Thomton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for Guidance Router task 


package GUIDANCE_ROUTER is 

task THE_GUIDANCE_ROUTER is 
entry CREATE; 

entry TAKE_HOMING_HEADING(HOMING_HEADING : in FLOAT: 

HOMING.MODE : in INTEGER): 
end THE_GUIDANCE_ROUTER: 

end GU1DANCE_R0UTER; 
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-Tide : guid_r_b.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Body for Guidance Router task 


with TEXT JO. NAVIGATOR.ROUTER; 
use TEXT JO, NAVlGATOR_ROUTER; 

package body GUIDANCE JIOUTER is 


-Task to handle routing of requests through Guidance 


task body THE_GUIDANCE_ROUTER is 

GUIDANCEJIEADING : FLOAT: 
GU1DANCE_M0DE: INTEGER: 


begin 

accept CREATE; 

PUT_LINE(' , Creating GUIDANCE ROUTER"); 
loop 

accept TAKE_HOMING_HEADING(HOMING_HEADING : in FLOAT: 

HOMING.MODE : in INTEGER) do 
GUIDANCE.HEADING := HOMING JIEADING: 

GUIDANCE.MODE := HOMING_MODE; 
end T AKE_HOMING_HEADING; 

THE_NAVIGATOR_ROUTER.TAKE_GUIDANCE_HEADING(GUIDANCE_HEADING, 

GUIDANCE.MODE): 

end loop: 

end THE_GUIDANCE_ROUTER: 
end GUIDANCE JROUTER; 
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—Tide 

--Author 

-Revised 

-Compiler 

-System 

-Description 


: gps_s.a (CLIPS-Ada Simulator Version) 
; F.P. Thornton Jr. 

: 26 Aug 93 
: VADS 
: Unix 

: Specification for GPS Control 


package GPS_CONTROL is 

task THE_GPS_CONTROL is 
entry CREATE; 

entry GET_GPS_F£X(G_FLAG_2 : out BOOLEAN); 
end THE_GPS_CONTROL: 

end GPS_CONTROL; 
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-Title : gps_b.a (CLIPS-Ada Simulator Version) 
-Author : FJ\ Thomton Jr. 


-Revised : 26 Aug 93 
-Compiler : VADS 
-System : Unix 
-Description : Body for GPS Control 


with TEXTJO; 
use TEXTJO; 

package body GPS_CONTROL is 
task body THE_GPS_CONTROL is 
begin 

accept CREATE; 

PUT_LINE(“Creating GPS CONTROL"); 
loop 

accept GET_GPS_FK(G_FLAG_2 : out BOOLEAN) do 
G_FLAG_2 :■ TRUE: 
end GET_GPS_FIX; 
end loop: 

end THE_GPS_CONTROL: 


end GPS_CONTROL; 
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--Tide : sonar_s.a (CLIPS-Ada Simulator Version) 

-Author : FJ\ Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

--Description : Specification for Sonar Control task 


package SONAR_CONTROL is 

task THE.SONAR.CONTROL is 
entry CREATE; 

entry DO_SEARCH_PATTERN(G_FLAG_2: out BOOLEAN; 
NAV_HEADENG; in FLOAT); 

entry UNKNOWN_OBSTACLE_P(G_FLAG_2. R_FLAG_2 : out BOOLEAN); 
entry LOG_NEW_OBSTACLE(G_FLAG_2; out BOOLEAN); 
end THE_SON AR_CONTROL; 

end SONAR_CONTROL: 
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-Title : sonar_b.a 

-Author : FJ\ Thornton Jr. 

-Revised : 26 Aug 93 
--Compiler : VADS 

-System : Unix 

-Description : Body for Sonar task 


with TEXT JO, MATH, CALENDAR. NAVIG ATOR.ROUTER, MISSION.MODEL, 
SENSOR Y.RECEIVER; 

use TEXT JO. MATH. CALENDAR. NAVIGATOR.ROUTER. MISSION.MODEL, 
SENSOR Y.RECEIVER; 

package body SONAR_CONTROL is 


-Task to handle Sonar Control functions including search, checking for 
-obstacles, and logging new obstacle position 


task body THE.SONAR.CONTROL is 
SECONDS : constant DURATION := 1.0; 

LEG.TIME : DURATION := 15 * SECONDS;-15 sec legs (+ 15 sec in turns) 
TURN_TIME ; constant DURATION := 15.0; 

INTERVAL : constant DURATION ;= 15 * SECONDS:--Amount to increase box 
NEXT TIME: TIME: 

LEG.NUM: INTEGER ;= 0: 

RANGEJ-IMIT : constant FLOAT := 300.0: -Limits for sonar in Search mode 
BEARING J.IMIT: constant FLOAT := PI / 3.0: 

SONAR.X: FLOAT; 

SONAR_Y: FLOAT; 

SONAR J)EPTH: FLOAT; 

SONAR.HEADING: FLOAT; 

SONAR.BEARING : FLOAT; 

SONAR_RANGE: FLOAT: 

SONAR.MODE : INTEGER := 2; 

SEARCH.HEADING: FLOAT; 

begin 

accept CREATE; 

PUT.LINECCreating SONAR CONTROL”); 
loop 
select 

-Do expanding box search pattern 

accept DO_SEARCH_PATTERN(G_FLAG_2 : out BOOLEAN; 

NAV.HEADING: in FLOAT) do 
SEARCH.HEADING := NAV.HEADING; 

NEXT.TIME := CLOCK + INTERVAL - TURN.TIME; 
loop 

if CLOCK > NEXT.TIME then -Change heading for new leg of search 
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if LEG_NUM s 2 then -Expand the box 
LEG.TIME :b LEG_TIME + INTERVAL: 

LEG.NUM := 1; 
end if; 

-Change heading to make box comer and normalize 
if (SEARCH_HEADING > (PI / 2.0)) then -Commanded heading > 0 
SEARCH.HEADING := SEARCH.HEADING - (PI / 2.0); 
else --Commanded heading <= 0 

SEARCH.HEADING SEARCH.HEADING + ((3.0 * PD / 2.0); 
end if; 

LEG.NUM := LEG.NUM + 1; 

NEXT.TIME := NEXT.TIME + LEG.TIME; 
end if; 

-Simulator protocol delay 
delay 0.3; 

THE_SENSORY_RECEIVER.RECEIVE(SON AR.X, SONAR.Y, SONAR.DEPTH. 
SONAR.HEADING. SONAR.BEARING, 

SONAR.RANGE); 

-Send commanded heading to Navigator 

THE_NAVIGATOR_ROUTER.TAKE_GUIDANCE_HEADING(SEARCH_HEADING. 

SON AR.MODE); 

-Check for valid range and bearing from sonar to end search 
exit when (SONAR.RANGE < RANGE.LIMIT and 
ABS(SONAR.BEARING) < BEARING.LIMIT); 
end loop; 

-Transition to Task mode 
SON AR.MODE := 3; 

THE_MISSION_MODEL.SET_MODE(SON’AR_MODE); 

G.FLAG.2 ;= TRUE: 
end DO.SEARCH.PATTERN; 
or 

accept UNKNOWN_OBSTACLE_P(G_FL AG_2. R.FLAG.2 : out BOOLEAN) do 
G.FLAG.2 ;= FALSE; 

R.FLAG.2 :« TRUE; 
end UNKNOWN.OBST ACLE.P: 
or 

accept LOG.NEW.OBSTACLE(G_FLAG.2 ; out BOOLEAN) do 
G.FLAG.2 :=TRUE; 
end LOG.NEW.OBSTACLE; 
end select; 
end loop; 

end THE.SONAR.CONTROL; 
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-Tide : replan_s.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thom toe Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Specification for Mission Replanner task 


package MJSSION_REPLANNER is 

task THE_M1SS10N_REPLANNER is 
entry CREATE; 

entry START_LOC AL_REPL ANNER; 
entry START_GLOBAL_REPLANNER; 
end THE_MISSION_REPLANNER: 

end MlSS!ON_REPLANNER: 
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--Title 

-Author 

-Revised 

-Compiler 

-System 

-Description 


: replan_b.a (CLIPS-Ada Simulator Version) 
: F J*. Thornton Jr, 

: 26 Aug 93 
: VADS 
: Unix 

: Body for Mission Replanner task 


with TEXT_IO, MISSION.MODEL, NAVIGATORJ^OUTER; 
use TEXT JO. MISSION.MODEL, NAVIGATOR_ROUTER; 

package body MISSIONJIEPLANNER is 


-Task to handle local and global replanning due to obstacles and system 
-faults 


task body THE_MISSION_REPLANNER is 
begin 

accept CREATE; 

PUTJ-INECCreating MISSION REPLANNER"): 
loop 
select 

accept STARTJ.OCAL_REPLANNER; 

-Delay to simulate replan time 
delay 30.0; 

THE_M1SS10N_M0DEL.SET_REPLAN_R0L , TE: 

THE_NAVIGATOR_ROUTER.REPLAN_DONE: 

or 

accept START_GLOBAL_REPLANNER; 

-Delay to simulate replan time 
delay 30.0; 

THE_MISSION_MODEL.SET_REPLAN_ROUTE: 
THE_NAVIGATOR_ROUTER.REPLAN_DONE: 
end select; 
end loop; 

end THE_M1SSI0N_REPLANNER: 
end MISSIONJREPLANNER: 
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--Title : los_s.a (CLIPS-Ada Simulator Version) 

-Author : FJ\ Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for LOS Calculator 


package LOS .CALCULATOR is 

procedure DO_LOS_GUIDANCE(FROM_X : in FLOAT; 
FROM.Y: in FLOAT; 

LOS.DEPTH : in out FLOAT; 

TO_X: in FLOAT; 

TO_Y: in FLOAT: 

TO.DEPTH; in FLOAT: 

LOS.SPEED: in FLOAT; 
LOS.HEADING : in out FLOAT): 

end LOS.CALCULATOR: 
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--Title : los_b.a (CLIPS-Ada Simulator Version) 
-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 

-Compiler : VADS 

-System : Unix 

-Description : Body for LOS Calculator 


with MATH. TR1G_MATH; 
use MATH. TRJG_MATH; 

package body LOS_CALCULATOR is 


—Procedure to calculate updated heading to next waypoint 


procedure DO_LOS_GUIDANCE(FROM_X : in FLOAT: 

FROM.Y : in FLOAT: 

LOS.DEPTH : in out FLOAT; 

TO_X: in FLOAT; 

TO_Y: in FLOAT; 

TO_DEPTH; in FLOAT. 

LOS.SPEED: in FLOAT: 

LOS.HEADING : in out FLOAT) is 
T1ME_0F_ARRIVAL: FLOAT; 

DELTA.TIME : FLOAT := 10.0: 
begin 

-Calculate updated heading to waypoint and normalize to 360 degrees 
LOSJHEADING := ATAN2((TO_X - FROM_X).(TO_Y - FROM_Y)); 
if LOS_HEADING < 0.0 then 
LOS.HEADING := LOS_HEADING + 2.0 * PI: 
end if; 

-Calculate updated depth 

TIME_OF_ARRIVAL ;= SQRT((TO_X - FROM_X)**2 + <TO_Y - FROM_Y)**2) / 
(LOSJSPEED/60.0); 

LOS.DEPTH := LOS.DEPTH + ({TO.DEPTH - LOS.DEPTH) * 

(DELTA _TIME / TIME.OF.ARRI V AL)): 
end DO.LOS.GUIDANCE; 

end LOS.CALCULATOR; 
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•-Title : homing_s.a (CLIPS - Ada Simulator V ersion) 

--Author : F.P. Thornton Jr. 

--Revised : 26 Aug 93 

-Compiler : VADS 

--System : Unix 

-Description : Specification for Homing Calculator 


package HOMING_CALCULATOR is 

procedure DO_HOMING_GUIDANCE(G_FLAG_3 : out BOOLEAN): 
end HOMING.CALCULATOR: 
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-Title : homing_b.a (CLIPS-Ada Simulator Version) 

-Author : FJ\ Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Body for Homing Calculator 


with TEXT 10, MATH. SENSORY_RECEIVER, GUIDANCE.ROUTER; 
use TEXT JO, MATH. SENSOR Y.RECEIVER, GUIDANCE J^OUTER; 

package body HOMING_CALCULATOR is 


-Procedure to calculate heading for homing 


procedure DO_HOMING_GUlDANCE(G_FLAG_3 : out BOOLEAN) is 
HOMING_X: FLOAT; 

HOMING. Y; FLOAT; 

HOMING.DEPTH: FLOAT; 

HOMING_HEADING ; FLOAT; 

HOMING.BEARING : FLOAT: 

HOMING JIANGE: FLOAT; 

HOMING_MODE : INTEGER := 3: -Initialize to task mode 

EPSILON : constant FLOAT := 20.0: -Tolerance for reaching target 
begin 
loop 

-Simulator protocol delay 
delay 0.5; 

THE_SENSORY_RECEIVER.RECEIVE(HOMING_X, HOMING_Y. HOMING.DEPTH, 
HOMING_HEADING, HOMING_BEARING. HOMING_RANGE); 
-Calculate updated heading to target 

HOMING_HEADING ;= HOMING.HE ADING + HOMING.BEARING; 

-Normalize heading to 360 degrees 
if HOMING JtE ADING < 0.0 then 
HOMING.HE ADING := HOMING_HEADING + (2.0 * PI); 
elsif HOMING_HEADING >= (2.0 * PI) then 
HOMING_HEADING := HOMING_HEADING - (2.0 * PI); 
else 
null: 
end if; 

-Send guidance commands to Guidance 

THE_GUIDANCE_ROUTER.TAKE_HOMING_HEADING(HOMING_HE ADING, 

HOMING.MODE): 

exit when HOMING JIANGE < EPSILON; 
end loop; 

G_FLAG_3 := TRUE; 
end DO_HOMING_GUIDANCE; 
end HONflNG.CALCULATOR; 
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-Title : miss_s.a 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for MISSION MODEL task 


package MISSION.MODEL is 

task THE_MISSION_MODEL is 
entry CREATE; 

entry INmALIZE(G_FLAG_l: out BOOLEAN); 

entry GIVE_FIRST_WAYPOINT(INmAL_X : out FLOAT: 

INITIAL.Y : out FLOAT; 

INITIAL.DEPTH; out FLOAT: 

INITIAL.MODE : out INTEGER: 

INITIALISEADING : out FLOAT; 

INITIAL.SPEED: out FLOAT; 

FIRST_WAYPOINT_X : out FLOAT; 
FIRSTWAYPOINT.Y : out FLOAT: 
FIRSTWAYPOINT.DEPTH: out FLOAT): 
entry IN.TRANS ITpIg.FL AG_ 1. R_FLAG_1 ; out BOOLEAN): 
entry TRANSIT_DONE_P(G_FLAG_l. R_FLAG_1 : out BOOLEAN): 
entry IN_SEARCH_P(G_FLAG_1, R_FLAG_1 : out BOOLEAN); 
entry SEARCH_DONE_P(G_FLAG_l, R_FLAG_1 : out BOOLEAN); 
entry IN_TASK_P(G_FLAG_1, R_FLAG_1 : out BOOLEAN); 
entry TASK_DONE_P(G_FLAG_ 1. R_FLAG_1 : out BOOLEAN): 
entry IN_RET17RN_P(G_FLAG_ 1. R_FLAG_ I: out BOOLEAN), 
entry RETURN_DONE_P(G_FLAG_ 1. R.FLAG_1 : out BOOLEAN): 
entry GIVE_NEXT_WAYPOINT(NEXT_X : out FLOAT; 

NEXT.Y : out FLOAT: 

NEXTjDEPTH : out FLOAT; 

NEXT.SPEED: out FLOAT: 

NEXT.MODE: out INTEGER); 
entry SET_REPLAN_ROUTE: 
entry SET_MODE(MISSION_MODE : in INTEGER): 
entry GET_MODE(MISSION_MODE : out INTEGER); 
end THE_MISSION_MODEL; 

end MISSION.MODEL: 
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-Title : missj>.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thomton Jr. 

-Revised : 28 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Body for Mission Model task 


with TEXTJO. NETWORK_SW; 
use TEXTJO, NETWORK.SW; 

package body MISSION_MODEL is 

package FLOATJNOUT is new FLOAT_IO(FLOAT); 
package INTEGER_INOUT is new INTEGER JO(INTEGER); 
use FLOATJNOUT, INTEGERJNOUT: 


-Task to manage mission database 


task body THE_MISSION_MODEL is 

INIT1 AL_STATE_FILE : FILE JYPE; 

WAYPOINTJFILE: FILE_TYPE: 

FINAL_GOALJTLE: FILE_TYPE: 

-Data structure to hold waypoints 
type WAYPOINT is 
record 
X : FLOAT; 

Y : FLOAT; 

DEPTH: FLOAT; 

HEADING : FLOAT; 

MODE: INTEGER; 

SPEED: FLOAT; 
end record; 

INITIAL : WAYPOINT; 

FINAL: WAYPOINT; 

MAX_WAYPOINTS : INTEGER := 25; 

type WAYPOINTS is array (INTEGER range 1 ..MAX.WAYPOINTS) of WAYPOINT; 
WAYPOINTJJST: WAYPOINTS; 

WAYPOINT.COUNT: INTEGER; 

I: INTEGER :* 1; -Counter for total number of waypoints 

K: INTEGER :* 1; -Counter for current waypoint 

CURRENT_MODE : INTEGER := 1; -Initialize mode to Transit 

begin 

accept CREATE; 

PUT.LINEC'Creating MISSION MODEL”); 
loop 
select 
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-Initialize Mission Model with initial state, waypoints, final goal 
accept INITIALIZE(G_FLAG_1: out BOOLEAN) do 
begin 

-Load initial state from file 

OPENdNITIAL_STATE.FELE, MODE => IN.FILE, NAME => “initiaLstate") 
GET(INITIAL_STATE_F[LE, INIT1AL.X); 

GET(INmAL_STATE_FILE, INITIAL.Y); 

GET(INmAL_STATE_FILE, INITIAL.DEPTH); 
GET(INmAL_STATE_FILE, tNTTIAL.HEADING); 
PUT.FLOATdNITlAL.X): 

PUT.FLOATONITIAL.Y); 

PUT_FLO AT(INTT1AL. DEPTH); 

PUT_FLOAT(INmAL.HEADING); 

CLOS EflNTTI AL_ST ATE_FILE); 


-Load waypoints from file 

OPEN(WAYPOINT_FILE. MODE => INJTLE, NAME => “waypoints"): 
GETCVVAYPOINTFILE. WAYPOINT_COUNT); 
SKIP_LINE(WAYPOINT_FILE); 

PUT_FLO AT (FLOAT (W A YPOINT_COUNT)): 
while not END_OF_FILE(WAYPOINT_FILE) loop 
GETfWAYPOINTJFILE. WAYPOINT_LIST(I).SPEED); 
GET(WAYPOINT_FILE. WAYPOINT_LIST(I).X): 
GET(WAYPOINT_FILE, WAYPOINT_LIST(I).Y): 
GET(WAYPOINT_FILE,WAYPOINT_LIST(I).DEPTH); 
GET(WAYPOINT_FILE, WAYPOINT_LIST(I).MODE): 

SKIP_LINE(W A YPOINT.FILE); 
PUT_FLOAT(WAYPOINT_LlST(I).SPEED); 
PUT_FLOAT(WAYPOINT_LIST(I).X): 
PUT_FLOAT(WAYPOINT_LlST(I).Y); 

PUT_FLO AT (W A YPOINT_LI ST(I). DEPTH); 

I := I + 1; 
end loop; 

CLOSEfW AYPOINT_FILE); 


-Load final goal from file 

OPEN(FINAL_GOAL_FILE. MODE => EN_FILE, NAME => “final_goal“); 
GET (FIN AL_GO AL_FILE. FINAL.X): 

GET (FIN AL_GO AL_FILE. FINAL. Y); 

PUT_FLOAT(FINAL.X): 

PUT_FLOAT(FINAL.Y); 

CLOSE(FIN AL.GO AL.FILE): 

GJFLAGJ ;= TRUE; 
exception 
when others => 

PUT_LINE(“Error in mission files"); 

G.FLAG _1 := FALSE; 
end: 

end INITIALIZE: 
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--Select initial state and first waypoint values 

accept GIVE_FTRST_WAYPOINT(INITIAL_X : out FLOAT; 

INITIAL.Y : out FLOAT; 

INITIAL.DEPTH: out FLOAT; 
INITIAL.MODE: out INTEGER; 

INITIAL.HEADING : out FLOAT; 
INITIAL.SPEED: out FLOAT; 

FIRST. WAYPOINT.X; out FLOAT; 
FIRST.WAYPOINT.Y: out FLOAT; 

FIRS T.WAYPOINT.DEPTH : out FLOAT) do 


INITIAL.X :* INITIAL.X; 

INITIAL.Y := INITIAL.Y; 

INITIAL.DEPTH:«INITIAL. DEPTH; 

INITIAL.HEADING :* INITIAL.HEADING; 

INITIAL.MODE ;= CURRENT.MODE; 

INITIAL.SPEED := WAYPOINT_LIST(l). SPEED; 

FIRST.WAYPOINT.X ;= WAYPOINT_LIST(l).X; 
FIRST.WAYPOINT.Y := WAYPOINT_LIST(l).Y; 
FIRST.WAYPOINT.DEPTH := WAYPOINT_LIST(l).DEPTH; 
end GIVE.FIRST.WAYPOINT; 
or 

-Entries to determine mission mode 
-Integer values equate to modes: 

- 1 = Transit, 2 = Search. 3 = Task, 4 = Return, 5 = Recover 


accept IN_TRANSIT_P(G_FLAG_1. R.FLAG.l : out BOOLEAN) do 
if CURRENT.MODE = 1 then 
G.FLAG.l := TRUE; 
else 

G.FLAG.l := FALSE: 
end if; 

R.FLAG.l := TRUE: 
end IN.TRANSIT.P; 
or 

accept TRANSIT_DONE_P(G_FLAG_l. R.FLAG.l : out BOOLEAN) do 
if CURRENT.MODE > 1 then 
G.FLAG.l := TRUE, 
else 

G.FLAG.l := FALSE; 
end if; 

R.FLAG.l := TRUE; 
end TRANSIT.DONE.P; 
or 

accept IN.SEARCH_P(G_FLAG. 1, R.FLAG.l ; out BOOLEAN) do 
if CURRENT.MODE = 2 then 
G.FLAG.l := TRUE; 
else 

G.FLAG.l := FALSE; 
end if: 

R.FLAG.l := TRUE; 
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end IN_SEARCH_P; 
x 

accept SEARCH_DONE_P(G_FLAG_l. R_FLAG_I : out BOOLEAN) do 
if CURRENT.MODE > 2 then 
G_FLAG_1 := TRUE; 
else 

G_FLAG_1 :* FALSE; 
end if; 

R_FLAG_1 := TRUE; 
end SEARCH_DONE_P; 
x 

accept IN_TASK_P(G_FLAG_ 1. R_FLAG_1 : out BOOLEAN) do 
if CURRENT_MODE * 3 then 
G_FLAG_1 := TRUE; 
else 

G_FLAG_1 ;= FALSE: 
end if; 

R_FLAG_1 := TRUE: 
end IN_TASK_P; 
ir 

accept TASK_DONE_P(G_FLAG_ 1. R_FLAG_1 : out BOOLEAN) do 
if CURRENTJMODE > 3 then 
G_FLAG_1 := TRUE; 
else 

G_FLAG_1 := FALSE; 
end if; 

R_FLAG_1 := TRUE: 
end TAS K_DONE_P: 

)r 

accept IN_RETURN_P(G_FLAG_1. R_FLAG_1 : out BOOLEAN) do 
if CURRENT_MODE = 4 then 
G_FLAG_1 := TRUE: 
else 

G_FLAG_1 := FALSE: 
end if: 

R_FLAG_1 := TRUE: 
end IN_RETURN_P; 

>r 

accept RETURN_DONE_P(G_FLAG_l. R_FLAG_1: out BOOLEAN) do 
if CURRENT.MODE > 4 then 
PUT_LINEC**********Goal Reached**********"); 

G_FLAG_1 := TRUE; 
else 

G_FLAG_1 := FALSE; 
end if; 

R_FLAG_1 := TRUE; 
end RETURN_DONE_P; 
x 

-Retrieve next waypoint for Navigator 

accept GIVE_NEXT_WAYPOINT(NEXT_X ; out FLOAT; 

NEXT.Y: out FLOAT: 
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NEXT.DEPTH: out FLOAT; 

NEXT.SPEED: out FLOAT; 

NEXT.MODE : out INTEGER) do 
NEXT.MODE := WAYPOINT_UST(K).MODE; 

NEXT_SPEED:» WAYPOINT_LIST(K).SPEED; 
if (CURRENT.MODE * 1) or (CURRENT.MODE = 2) or 
(CURRENT_MODE * 4) then -Normal case:use next waypoint X.YDEPTH 
NEXT.X := WAYPOINT_UST(K + 1).X; 

NEXT.Y :« WAYPOINT_LlST(K + 1).Y; 

NEXT.DEPTH := WAYFOINT_LIST(K + 1).DEPTH; 

CURRENT.MODE :■ WAYPOINT LIST(K).MODE; 

K :* K + 1; 

else -Odd case:use current waypoint X.YDEPTH 
NEXT.X :* WAYPOINT_LIST(K).X; 

NEXT.Y ;* WAYPOINT_LIST(K).Y; 

NEXT.DEPTH := WAYPOINT_LIST(K)DEPTH; 

CURRENT.MODE := WAYPOINT_LIST(K).MODE; 
end if; 

end G1VE.NE XT. WAYPOINT; 
or 

-Change waypoint, mode, and speed for replan route 
accept SET.REPLAN.ROUTE do 
K := 1 - 3; 

WAYPOINT_LIST(K).MODE := 4; 

WAYPOINT_LIST(K).SPEED WAYPOINT.LISTfK + D.SPEED; 
end SET.REPLAN.ROUTE; 
or 

accept SET_M0DE(MlSSlON_M0DE : in INTEGER) do 
CURRENT.MODE := M1SSION.MODE: 
end SET.MODE; 
or 

accept GET_MODE(MISSION_MODE : out INTEGER) do 
MISSION.MODE := CURRENT.MODE: 
end GET.MODE: 
end select; 
end loop; 

end THE.MISSION.MODEL: 
end MISSION.MODEL: 
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--Title : world_s.a (CLIPS-Ada Simulator Version) 

--Author : FP. Thornton Jr. 

--Revised : 26 Aug 93 
--Compiler : VADS 

-System : Unix 

-Description : Specification for World Model task 


package WORLD_MODEL is 

task THE_WORLD_MODEL is 
entry CREATE; 

entry IMT1ALIZE(G_FLAG_1 : out BOOLEAN); 
entry GET_SONAR_CONTACT(SONAR_X : out FLOAT; 
SONAR. Y: out FLOAT); 

entry ADD_OBSTACLE(OBSTACLE_X : in FLOAT; 
OBSTACLE. Y: in FLOAT. 
OBSTACLE.DEPTH : in FLOAT): 
end THE.WORLD.MODEL; 

end WORLD.MODEL; 
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-Title 

-Author 

-Revised 

-Compiler 

-System 

-Description 


: world_b.a (CLIPS-Ada Simulator Version) 
: FP. Thornton Jr. 

: 26 Aug 93 
: VADS 
: Unix 

: Body for World Model task 


with TEXT_IO, NETWORK.SW; 
use TEXT JO, NETWORK.SW; 

package body WORLD.MODEL is 
package FLOATJNOUT is new FLOAT.IOCFLOAT); 
package INTEGER.INOUT is new INTEGER JO(INTEGER); 
use FLOAT JNOUT. INTEGER.INOUT; 


-Task to manage world database, which includes obstacles 


task body THE.WORLD.MODEL is 

OBSTACLE.FILE: FILE.TYPE; 

-Data structure to hold obstacles 
type OBSTACLE is 
record 
X : FLOAT; 

Y : FLOAT; 

DEPTH : FLOAT; 
end record; 

CURRENT.OBSTACLE: OBSTACLE; 

NEXT.OBSTACLE; OBSTACLE; 

MAX.OBSTACLES : INTEGER := 25: 

type OBSTACLES is array (INTEGER range 1..MAX.OBSTACLES) of OBSTACLE: 
OBSTACLE.L1ST: OBSTACLES: 

OBSTACLE.COUNT: INTEGER: 

J: INTEGER := 1; -Counter for total number of obstacles 

begin 

accept CREATE; 

PUT.LINECCreating WORLD MODEL"); 
loop 
select 

-Initialize World Model by loading obstacles 
accept INlTlALIZE(G_FLAGJ : out BOOLEAN) do 
begin 

OPEN(OB STACLE.FILE. MODE => IN.FILE, NAME => "obstacles”): 
GET(OBSTACLE_FILE. OBSTACLE.COUNT); 
SKIP_LINE(OBSTACLE_FILE); 
PUT_FLOAT(FLOAT(OBSTACLE_COUNT)); 
while not END_OF_FILE( OB ST ACLE.FILE) loop 
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GET(OBSTACLE_FILE, OBSTACLE UST(J).X); 
GET(OBSTACLE_FILE, OBSTACLE_UST(J).Y); 
GET(OBSTACLE_FILE, OBSTACLE_LIST(J).DEFIH): 
SK1PJLINE(0BSTACLE_FILE); 

PUT_FLOAT(OBSTACLE LIST(J).X); 
PUT_FLOAT(OBSTACLE_UST(J).Y); 
PUT_FLOAT(OBSTACLE_UST(J).DEPTH); 

J := J + 1; 
end loop; 

CLOSE(OBSTACLE_FILE); 

NEXT.OBSTACLE:« 0BSTACLE_L1ST(J); 

G_FLAG_1 ;« TRUE; 
exception 
when others *> 

PUT_LINE("EiTor in world files**); 

G_FLAG_1 := FALSE: 
end; 

end INITIALIZE; 
or 

-Get an obstacle for sonar target 

accept GET_SONAR_CONTACT(SONAR_X : out FLOAT: 

SONAR_Y : out FLOAT) do 
SONAR.X := 0BSTACLE_L1ST(1).X; 

SONAR.Y := OBSTACLE_LIST( 1 ).Y; 
end GET_SONAR_CONTACT; 
or 

-Add a new obstacle 

accept ADD_OBSTACLE(OBSTACLE_X : in FLOAT: 
OBSTACLE. Y: in FLOAT; 
OBSTACLE.DEPTH : in FLOAT) do 
NEXT_OBSTACLE.X := OBSTACLE.X; 
NEXT_OBSTACLE.Y := OBSTACLE_Y; 
NEXT.OBSTACLE. DEPTH ;= OBSTACLE.DEPTH; 
NEXT.OBSTACLE ;= OBSTACLE_LIST(J); 

J := J + 1; 

end ADD.OBSTACLE: 
end select: 
end loop; 

end THE_WORLD_MODEL; 
end WORLD.MODEL: 
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--Title : receiv_s.a (CLIPS-Ada Simulator Version) 

-Author : FP. Thomton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Specification for Sensory Receiver task 


package SENSORY.RECEIVER is 

task THE.SENSORY.RECEIVER is 
entry CREATE; 

entry RECEIVE(CURRENT_X : in out FLOAT; 
CURRENT.Y : in out FLOAT; 
CURRENT.DEPTH : in out FLOAT; 
CURRENT.HEADING : in out FLOAT; 
CURRENT.BEARDMG : in out FLOAT: 
CURRENT.RANGE : in out FLOAT): 
end THE_SENS0RY_RECE1VER; 

end SENSORY.RECEIVER: 
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-Title : receiv_b.a 

-Author : FP. Thornton Jr. 

-Revised : 26 Aug 93 
-Compiler : VADS 

-System : Unix 

-Description : Body for Sensory Receiver task 


with TEXT JO. MATH. TRIG.MATH. NETWORK.SW, WORLD.MODEL; 
use TEXT JO. MATH. TRIG.MATH. NETWORK.SW, WORLD.MODEL; 

package body SENSORY.RECEIVER is 

package FLOAT.INOUT is new FLOATJO(FLOAT); 
package INTEGER_INOUT is new INTEGER JO{INTEGER): 
use FLOAT.INOUT. INTEGER.INOUT; 


-Task to get navigation stams values from execution level and provide 
-them to the tactical level 


task body THE.SENSORY.RECEIVER is 

RECEIVED : BOOLEAN := FALSE. 
CURRENT.ALT: FLOAT; 

CURRENT.SPEED: FLOAT: 

SONAR.X: FLOAT: 

SONAR.Y : FLOAT: 

begin 

accept CREATE: 

PUT_LINE("Creating SENSORY RECEIVER"): 
loop 

accept RECEIVE(CURRENT_X : in out FLOAT; 
CURRENT.Y : in out FLOAT: 
CURRENT.DEPTH : in out FLOAT: 
CURRENT.HEADING : in out FLOAT. 
CURRENT.BEARING : in out FLOAT; 
CURRENT.RANGE : in out FLOAT) do 
CURRENT.X := get.float: 

PUTCCurrent X = ”); 

PUT(CURRENT_X. FORE=>3. AFT=>2EXP=>0); 
NEW.LINE; 

CURRENT. Y := get.noat; 

PUT(“Current Y * "); 

PUT(CURRENT_Y. FORE=>3. AFT=>2EXP=>0): 
NEW.LINE; 

CURRENT.ALT := get.float; 
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CURRENT.DEPTH:» get.float; 

PUTCCurrent Depth * "); 

PUT(CURRE NT_DEPTH. FORE«>3. AFT«>2£XP»>0); 

NEW.LINE; 

CURRENT.HEADING:« DEG_TO_RAD(get_float); 

PUT(“Cunent Heading * “); 

PUT(RAD_TO_DEG(CURRENT_HEADING). FORE«>3. AFT«>2.EXP=>0); 
NEW.LINE; 

-Speed does not come firom the simulator 
CURRENT.SPEED;» 0.0; 

-Calculate bearing and range to simulated sonar contact 
if not RECEIVED then 

THE_WORLD_MODEL.GET_SONAR_CONTACT(SONAR_X. SONAR Y); 
RECEIVED := TRUE; 
end if; 

CURRENT_BEARING ;= CURRENT.HEADING - 

ATAN2((SONAR_X - CURRENT_X).(SONAR_Y - CURRENT_Y)); 
—Normalize to 360 degrees but keep negative values for bearing 
if CURRENT.BE ARING < 0.0 then 
CURRENT.BEARING := ABS(CURRENT_BEARING); 
elsif CURRENT.BEARING > PI then 
CURRENT.BE ARING ;= (2.0 * PI) - CURRENT.BEARING; 
else -0.0 <= CURRENT.BEARING <= PI 
CURRENT.BEARING ;= 0.0 - CURRENT.BEARING: 
end if; 

PUTCCurrent Bearing = “); 

PUT(RAD_TO_DEG(CURRENT.BE ARING). FORE=>3. AFT=>2. EXP=> 0); 
NEW.LINE; 

CURRENT.RANGE := SQRT«SONAR_X - CURRENT_X)**2 + 

(SONAR.Y - CURRENT.Y)* *2): 

PUTCCurrent Range = “); 

PUT(CURRENT_RANGE. FORE=>3. AFT=>2. EXP=>0); 

NEW.LINE: 
end RECEIVE; 
end loop; 

end THE.SENSORY.RECEIVER; 
end SENSORY.RECEIVER: 


108 





-Title 

-Author 

-Revised 

-Compiler 

-System 

-Description 


: trig_mth.a 
: R.B. Bymes 

: 18 Aug 93 by F.P. Thornton Jr. 

:VADS 
: Unix 

: Trigonometric and conversion functions 


with MATH; 
use MATH; 

package TRIG_MATH is 
LOWER_LIMJT: constant FLOAT := 0.000001; 
function ATAN2(Y,X ; FLOAT) return FLOAT; 
function RAD_TO_DEG(X; FLOAT) return FLOAT; 
function DEG_TO_RAD{X ; FLOAT) return FLOAT; 
end TRIG_MATH; 

package body TRIG_MATH is 


-Trig functions for heading and bearing calculations 


function SIGNUM (R : FLOAT) return FLOAT is 
begin 

if R <0.0 then 
return -1.0; 
else 

return +1.0; 
end if; 

end SIGNUM; 

function ATAN2(Y.X : FLOAT) return FLOAT is 
begin 

if ABS(X) > LOWER.LIMIT then 
if X > 0.0 then 
return ATAN(Y/X); 
else 

return ATAN(Y/X) + (SIGNUM(Y) • PI); 
end if; 
else 

return SIGNUM(Y) * (PI/2.0); 
end if; 

end ATAN2: 


-Conversion functions for angles 


function RAD_TO_DEG(X: FLOAT) return FLOAT is 
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begin 

return X* (180.0 /PI); 
end RAD_TO_DEG; 

function DEG_TO_RAD(X : FLOAT) return FLOAT is 
begin 

return X * (PI / 180.0); 
end DEG_TO_RAD; 

end TRIG.MATH; 
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-Title : netwkj.a 

-Author : R.B. Byrnes 

-Revised : 30 Jul 93 by F.P. Thornton Jr. 

-Compiler : VADS 

-System : Unix 

-Description : Interface to C communication routines 


package NETWORK_SW is 

- CLIENT comms. supporting Tactical<->Execution level 

procedure statt_comms; - make connection to E-level 
procedure put_float (X : FLOAT): - send float to E-level 
function get_float return FLOAT; - receive float from E-Jevel 
procedure put_mode (X : INTEGER): - send mode to E-level 
procedure stop_comms: - close connection to E-level 

- System clock access function. Better than Ada's 
procedure getjime: 

private 

pragma INTERFACE(C, start_comms); 
pragma INTERFACE(C. put_float); 
pragma INTERFACE(C. get_float); 
pragma INTERFACE(C. stop_comms); 
pragma ENTERFACEfC. put_mode): 
pragma INTERFACE(C, get_time); 

pragma LINK_WITH("network_sw.o"); - lump all above files together 
end NETWORK_SW; 
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APPENDIX B. TRACES OF MISSION EXECUTION 


1. MULTI-PHASE MISSION 

CLIPS> (assert (start)) 

CLIPS> (run) 

Creating OOD 
Creating MISSION MODEL 
Creating WORLD MODEL 
Creating SENSORY RECEIVER 
Creating OOD ROUTER 
Creating NAVIGATOR 
Creating ENGINEERING 
Creating WEAPONS 
Creating NAVIGATOR ROUTER 
Creating GUIDANCE 
Creating GPS CONTROL 
Creating MISSION REPLANNER 
Creating SONAR CONTROL 
Creating GUIDANCE ROUTER 

READY_VEHICLE_FOR_LAUNCH GOAL FLAG = 1 

SELECT_FIRST_WAYPOINT GOAL FLAG = 1 

IN_TRANSIT_P GOAL FLAG = 1 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 
GPS_NEEDED_P GOAL FLAG = 0 

REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful! 
UNKNOWN_OBSTACLE_P GOAL FLAG = 0 

Commanded Heading is: 45.00 
Commanded Depth is: 5.89 

Commanded Speed is: 250.00 
Commanded X is: 250.00 
Commanded Y is: 250.00 
Commanded Mode is: Transit 
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SEND_SETPOINTS_AND_MODES GOAL FLAG = 
TRANSIT_DONE_P GOAL FLAG = 0 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 1 

TRANSIT_DONE_P GOAL FLAG = 0 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROE_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 
GPS_NEEDED_P GOAL FLAG = 0 

Current X = 8.81 

Current Y = 0.00 

Current Depth = -0.00 

Current Heading = 89.00 

Current Bearing = -21.92 
Current Range = 641.87 
REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful 
Commanded Heading is: 43.97 
Commanded Depth is: 6.00 

Commanded Speed is: 250.00 
Commanded X is: 250.00 
Commanded Y is: 250.00 
Commanded Mode is: Transit 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 1 

TRANSIT_DONE_P GOAL FLAG = 0 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB P GOAL FLAG = 0 
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STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 

GPS__NEEDED_P GOAL FLAG = 0 

Current X = 17.39 

Current Y = -0.05 

Current Depth = -0.01 

Current Heading = 88.00 

Current Bearing = -21.23 

Current Range = 634.00 

REACH_WA Y POINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY__SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful 

Commanded Heading is: 42.93 

Commanded Depth is: 6.09 

Commanded Speed is: 250.00 

Commanded X is: 250.00 

Commanded Y is: 250.00 

Commanded Mode is: Transit 


GPS_NEEDED_P GOAL FLAG = 0 

Current X = 240.39 
Current Y = 234.65 
Current Depth = 48.17 

Current Heading = 32.00 

Current Bearing = 55.56 

Current Range = 359.94 
REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful 
Commanded Heading is: 32.04 
Commanded Depth is: 52.38 
Commanded Speed is: 250.00 
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Commanded X is: 250.00 
Commanded Y is: 250.00 
Commanded Mode is: Transit 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 1 

TRANSIT_DONE_P GOAL FLAG = 0 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

o crit-system-prob branch successful! 

GPS_NEEDED_P GOAL FLAG = 0 

*****At waypoint, coming to new heading****’ 

Current X = 245.06 

Current Y = 241.97 

Current Depth = 49.04 

Current Heading = 32.00 

Current Bearing = 56.70 

Current Range = 355.04 

REACH_WAYPOINT_P GOAL FLAG = 1 

GET_NEXT_WAYPOINT GOAL FLAG = 1 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful! 

Commanded Heading is: 31.61 

Commanded Depth is: 53.26 

Commanded Speed is: 250.00 

Commanded X is: 450.00 

Commanded Y is: 150.00 

Commanded Mode is: Search 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 1 

Current X = 249.73 
Current Y = 249.32 
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Current Depth = 49.82 

Current Heading = 32.00 

Current Bearing = 57.89 

Current Range = 350.27 
Commanded Heading is: 31.61 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Search 

Current X = 254.40 
Current Y = 256.71 
Current Depth = 50.51 

Current Heading = 32.00 

Current Bearing = 59.11 

Current Range = 345.66 
Commanded Heading is: 31.61 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Search 

Current X = 259.08 
Current Y = 264.11 
Current Depth = 51.21 

Current Heading = 32.00 

Current Bearing = 60.37 

Current Range = 341.22 
Commanded Heading is: 31.61 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Search 


Current X = 301.47 
Current Y = 222.07 
Current Depth = 54.45 
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Current Heading = 123.00 
Current Bearing = -38.35 
Current Range = 299.84 
Commanded Heading is: 121.61 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Search 

DO_SEARCH_PATTERN GOAL FLAG 
SEARCH DONE P GOAL FLAG = 


* SEARCH SUCCESSFUL. 


IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 1 

Current X = 308.74 
Current Y = 217.49 
Current Depth = 54.45 

Current Heading = 123.00 
Current Bearing = -39.37 
Current Range = 293.07 
Commanded Heading is: 83.63 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Task 


Current X = 316.03 
Current Y = 212.91 
Current Depth = 54.45 

Current Heading = 123.00 
Current Bearing = -40.44 
Current Range = 286.38 
Commanded Heading is: 82.56 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Task 








Current X = 323.31 
Current Y = 208.28 
Current Depth = 54.45 

Current Heading = 121.00 
Current Bearing = -39.58 
Current Range = 279.82 
Commanded Heading is: 81.42 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Task 


Current X = 576.69 
Current Y = 243.88 
Current Depth = 56.06 

Current Heading = 76.00 

Current Bearing = -0.71 

Current Range = 24.10 

Commanded Heading is: 75.29 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Task 

Current X = 585.25 
Current Y = 246.07 
Current Depth = 56.06 

Current Heading = 76.00 

Current Bearing = -0.93 

Current Range = 15.27 

Commanded Heading is: 75.07 
Commanded Depth is: 53.26 
Commanded Speed is: 250.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Task 

HOMING GOAL FLAG = 1 
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DROP_PACKAGE GOAL FLAG = 1 

GET_GPS_FIX GOAL FLAG = 1 

GET_NEXT_WAYPOINT GOAL FLAG = 1 

TASK_DONE_P GOAL FLAG = 1 


* TASK SUCCESSFUL. 


IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 1 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 

GPS_NEEDED_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 0 

Current X = 593.81 

Current Y = 248.26 

Current Depth = 56.06 

Current Heading = 76.00 

Current Bearing = -1.74 

Current Range = 6.43 

REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful! 

Commanded Heading is: 235.66 

Commanded Depth is: 47.08 

Commanded Speed is: 360.00 

Commanded X is: 450.00 

Commanded Y is: 150.00 

Commanded Mode is: Return 


SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

RETURN_DONE_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 1 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER SYSTEM_PROB_P GOAL FLAG = 0 
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PROPULSION_SySTEM_PROB_P GOAL FLAG = 
STEERING_SYSTEM PROB P GOAL FLAG = 0 


No crit-system-prob branch successful! 


GPS_NEEDED_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 0 

Current X = 602.39 

Current Y = 250.45 

Current Depth = 56.06 

Current Heading = 76.00 

Current Bearing = -176.59 

Current Range = 2.43 

REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD PROB P GOAL FLAG = 0 


No red-cap-system-prob branch successful 

Commanded Heading is: 236.61 

Commanded Depth is: 47.49 

Commanded Speed is: 360.00 

Commanded X is: 450.00 

Commanded Y is: 150.00 

Commanded Mode is: Return 


GPS_NEEDED_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 0 

Current X = 308.61 

Current Y = 43.42 

Current Depth = 20.17 

Current Heading = 220.00 

Current Bearing = -165.33 

Current Range = 357.18 

REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful 


120 






Commanded Heading is: 212.68 
Commanded Depth is: 19.53 
Commanded Speed is: 360.00 
Commanded X is: 300.00 
Commanded Y is: 30.00 
Commanded Mode is: Return 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

RETURN_DONE_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 1 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 

GPS_NEEDED_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 0 

*****At waypoint, coming to new heading***** 

Current X = 300.79 

Current Y = 34.16 

Current Depth = 19.81 

Current Heading = 217.00 

Current Bearing = -162.81 

Current Range = 368.93 

REACH_WAYPOINT_P GOAL FLAG = 1 

GET_NEXT_WAYPOINT GOAL FLAG = 1 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB_P GOAL FLAG = 0 

No red-cap-system-prob branch successful! 

Commanded Heading is: 190.82 

Commanded Depth is: 22.45 

Commanded Speed is: 360.00 

Commanded X is: 0.00 

Commanded Y is: 0.00 

Commanded Mode is: Recover 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN SEARCH_P GOAL FLAG = 0 
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IN_TASK_P GOAL FLAG = 0 

»*********G 0a i Reached********** 

RETURN_DONE_P GOAL FLAG = 1 

IN_RETURN_P GOAL FLAG = 0 

WAIT_FOR_RECOVERY GOAL FLAG = 1 

**************.*************** Current x = 293.27 

Current Y = 24.58 

Current Depth = 19.54 

Current Heading = 214.00 

Current Bearing = -160.31 

Current Range = 380.66 
***** 

* RETURN SUCCESSFUL. 


******♦Commanded Heading is: 214.00 
Commanded Depth is: 0.00 

Commanded Speed is: 0.00 

Commanded X is: 0.00 

Commanded Y is: 0.00 

Commanded Mode is: Recover 


**************************** 

* MISSION EXECUTED SUCCESSFULLY. * 

* AUV IS WAITING FOR RECOVERY... * 

* ***••••••***«**••••*•«•.*.c urre r.t X = 286.27 

Current Y = 14.60 

Current Depth = 19.32 

Current Heading = 208.00 
Current Bearing = -154.88 
Current Range = 392.22 
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MULTI-PHASE MISSION WITH ROUTE REPLANNING 


CLIPS> (assert (start)) 

CLIPS> (run) 

Creating OOD 
Creating MISSION MODEL 
Creating WORLD MODEL 
Creating SENSORY RECEIVER 
Creating OOD ROUTER 
Creating NAVIGATOR 
Creating ENGINEERING 
Creating WEAPONS 
Creating NAVIGATOR ROUTER 
Creating GUIDANCE 
Creating GPS CONTROL 
Creating MISSION REPLANNER 
Creating SONAR CONTROL 
Creating GUIDANCE ROUTER 

READY_VEHICLE_FOR_LAUNCH GOAL FLAG = 1 

SELECT_FIRST_WAYPOINT GOAL FLAG = 1 

WARNING: Reset Command may not be performed during the 

execution of a rule 

IN_TRANSIT_P GOAL FLAG = 1 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 

GPS_NEEDED_P GOAL FLAG = 0 

REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P GOAL FLAG = 0 

PAYLOAD_PROB__P GOAL FLAG = 0 

No red-cap-system-prob branch successful! 

UNKNOWN_OBSTACLE_P GOAL FLAG = 0 

Commanded Heading is: 45.00 

Commanded Depth is: 5.89 

Commanded Speed is: 250.00 

Commanded X is: 250.00 

Commanded Y is: 250.00 

Commanded Mode is: Transit 
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SEND_SETPOINTS_AND_MODES GOAL FLAG = 
TRANSIT_DONE_P GOAL FLAG = 0 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 1 

TRANSIT_DONE_P GOAL FLAG = 0 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 
Current X = 8.81 

Current Y = 0.00 

Current Depth = - 0.00 

Current Heading = 89.00 

Current Bearing = -21.92 
Current Range = 641.87 
REACH_WAYPOINT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 0 

LEAK_TEST_P CoA« FLAG = 0 

PAYLOAD_PP.QP_P GOAL FLAG = C 
No red-cap-system-prob branch successful 
Commanded Heading is: 43.97 
Commanded Depth is: 6.00 

Commanded Speed is: 250.00 
Commanded X is: 250.00 
Commanded Y is: 250.00 
Commanded Mode is: Transit 


Current X = 124.75 

Current Y = 81.84 

Current Depth = 18.64 

Current Heading = 38.00 

Current Bearing = 32.51 

Current Range = 504.12 

REACH_WAYP01NT_P GOAL FLAG = 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 
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BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 1 

Commanded Heading is: 36.68 
Commanded Depth is: 24.87 
Commanded Speed is: 250.00 
Commanded X is: 250.00 
Commanded Y is: 250.00 
Commanded Mode is: Transit 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

LOITER GOAL FLAG = 1 

Current X = 129.81 

Current Y = 88.16 

Current Depth = 19.87 

Current Heading = 38.00 

Current Bearing = 33.01 

Current Range = 497.26 

Commanded Heading is: 218.71 

Commanded Depth is: 24.87 

Commanded Speed is: 250.00 

Commanded X is: 124.75 

Commanded Y is: 81.84 

Commanded Mode is: Transit 

Current X = 134.89 
Current Y = 94.49 

Current Depth = 21.11 

Current Heading = 38.00 

Current Bearing = 33.51 

Current Range = 490.42 
Commanded Heading is: 218.73 
Commanded Depth is: 24.87 
Commanded Speed is: 250.00 
Commanded X is: 124.75 
Commanded Y is: 81.84 
Commanded Mode is: Transit 

Current X = 140.03 
Current Y = 100.77 
Current Depth = 22.37 

Current Heading = 36.00 

Current Bearing = 36.02 

Current Range = 483.57 
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Commanded Heading is: 218.92 
Commanded Depth is: 24.87 
Commanded Speed is: 250.00 
Commanded X is: 124.75 
Commanded Y is: 81.84 
Commanded Mode is: Transit 


Current X = 241.34 

Current Y = 237.36 

Current Depth = 48.08 

Current Heading = 35.00 

Current Bearing = 52.98 

Current Range = 358.88 

REACH_WAYPOINT_P GOAL FLAG - 0 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 1 

Commanded Heading is: 34.40 

Commanded Depth is: 53.30 

Commanded Speed is: 250.00 

Commanded X is: 250.00 

Commanded Y is: 250.00 

Commanded Mode is: Transit 

SEND_SETPOINTS_AND_MODES GOAL FLAG = 1 

IN_SEARCH_P GOAL FLAG = 0 

IN_TASK_P GOAL FLAG = 0 

IN_RETURN_P GOAL FLAG = 0 

IN_TRANSIT_P GOAL FLAG = 1 

TRANSIT_DONE_P GOAL FLAG = 0 

POWER_GONE_P GOAL FLAG = 0 

COMPUTER_SYSTEM_PROB_P GOAL FLAG = 0 

PROPULSION_SYSTEM_PROB_P GOAL FLAG = 0 

STEERING_SYSTEM_PROB_P GOAL FLAG = 0 

No crit-system-prob branch successful! 
♦****At waypoint, coming to new heading***** 
Current X = 245.89 
REACH_WAYPOINT_P GOAL FLAG = 1 

Current Y = 244.09 
Current Depth = 48.99 
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Current Heading = 35.00 

Current Bearing = 54.04 

Current Range = 354.16 
GET_NEXT_WAYPOINT GOAL FLAG = 1 

DIVING_SYSTEM_PROBLEM_P GOAL FLAG = 0 

BUOYANCY_SYSTEM_PROB_P GOAL FLAG = 0 

THRUSTER_SYSTEM_PROB_P GOAL FLAG = 1 

Commanded Heading is: 34.83 
Commanded Depth is: 54.84 
Commanded Speed is: 360.00 
Commanded X is: 450.00 
Commanded Y is: 150.00 
Commanded Mode is: Return 
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APPENDIX C. AUV SIMULATOR USER’S GUIDE 

To run the AUV simulator, the following is required: a file with a set of CLIPS 
rules, an executable file for CLIPS-Ada, an executable file for the AUV graphical 
simulator, and four data files for inputs to the simulator. The CLIPS rule file serves as the 
Strategic level. The executable file for CLIPS-Ada allows the CLIPS rules to call the 
Tactical level procedures. The executable file for the graphical simulator acts as the 
Execution level as well as the physical vehicle itself. The four data files for input are 
“initial_state”, “waypoints”, “final_goal”, and “obstacles”. These files must be initialized 
first. 

Data is entered into the “initial_state” file in the format illustrated in Figure 1. 



Data is entered into the “waypoints” file in the format illustrated in Figure 2. 


3^1 -_-Number of Waypoints 


250.0 100.0 100.0 20.0 2 
300.0 200.0 150.0 30.0 4 
300.0 50.0 50.0 10.0 5^ 

Speed X Y Depth Mode 


Mode key: 

1 = Transit 

2 = Search 

4 = Return 

5 = Recover 


Figure 2 “waypoints” Data File 
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Data is entered into the “fmal_goal” file in the format shown in Figure 3. 



Data is entered into the “obstacles” file in the format shown in Figure4. 



Once the data files are set up, the simulator can be run from any Silicon Graphics 
workstation in the Graphics laboratory. First, two window shells must be called up- the first 
to run the Execution level/graphical simulator and the second to run the Strategic/Tactical 
level. In the first window, the executable file “auv2” is run. In the second window, an rlogin 
to Virgo must be done and then either the “str_tac 1 ” (multi-phase mission) or the “str_tac2” 
(multi-phase mission with route replanning) executable file for CLIPS-Ada must be run. At 
the prompt, the host name is entered as “iris/i”. Then the appropriate CLIPS rule set is 
loaded by entering “(lead strlevx”). Finally, to start the simulation, a “start” fact must be 
asserted (“(assert (start))”) and the run command must be given (“(run)”). The simulation 
can be stopped by killing the “auv2” process. 
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